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Abstract 

--A  method  of  calculating  mechanical  efficiency  was  developed  as  a  means 
of  comparing  the  performance  of  different  types  of  manipulators.  As  an  initial 
approach  to  this  problem  takes  into  account  inertial  and  gravitational  terms  of 
the  robot  configurations  in  addition  to  a  variable  payload.  The  method  included 
developing  a  numerical  integration  algorithm  to  calculate  the  work  done  by  each 
manipulator  at  any  point  in  that  manipulator’s  workspace.  The  efficiencies  of 
two  robotic  manipulator  configurations  that  are  candidates  for  the  design  of  the 
AFIT,  AAMRL,  Anthropomorphic  Robotic  Manipulator,  (A^RAI),  were  analyzed. 
The  two  designs  were  a  serial  open  Unk  direct  drive  manipulator,  and  the  closed 
parallel  kinematic  chain  direct  drive  manipulator  design  by  Dr.  Asada  at  M.  I.  T. 
The  difference  between  the  mainpulators  was  actual  mass  and  kinematic  design. 

The  efficiency  measure  used  to  analyze  both  manipulators  was  based  on  the 
magnitude  of  the  total  work  done  by  the  manipulator  to  move  a  payload  a  pre¬ 
scribed  distance.  The  effects  of  a  variable  mass  payload  on  efficiency  have  now 

T' 

been  individually  examined  for  the  cases  when  the  arm  has  been  '‘tuned”  for  some 
nominal  payload  by  means  of  compensating  for  gravity,  making  the  robotic  config¬ 
uration  invariant,  and  decoupUng  the  manipulator’s  dynamic  equations  of  motion. 

An  algorithm  was  developed  for  calculating  the  mechanical  efficiency  for  dif¬ 
ferent  robotic  mainipulator  configurations.  When  the  manipulators  are  gravity 
compensated  for  a  nominal  payload,  their  efficiency  increases  dramatically,  even 
when  the  payload  is  varied  from  nominal.  In  addition,  when  the  configuration  is 
tuned  for  dynamically  decoupling  and  configuration  invarianccf  efficiency  is  im¬ 
proved.  Finally,  for  most  of  the  reachable  workspace  <rf  the  manipulators,  the 
parallel  manipulator  is  the  most  efficient. 
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CONFIGURATION  COMPARISION  ANALYSIS  FOR  THE 

AFIT/AAMRL 

ANTHROPOMORPHIC  ROBOTIC  MANIPULATOR 

I.  Introduction 

A  method  was  needed  to  analyze  the  performance  of  different  robotic  ma¬ 
nipulator  configurations.  This  method  will  be  used  to  select  the  configuration 
used  for  the  AFIT / A AMRL  anthropomorphic  robotic  manipulator  {A^RAI),  which 
will  be  used  for,  (one  appUcation),  research  in  human  telepresence.  The  goal  of 
the  {A^RM)  is  a  manipulator  system  capable  of  achieving  human  levels  of  reach, 
speed,  and  load  carrying  capacity.  An  analysis  of  two  mechanical  configurations 
for  the  design  of  a  general  two  degree  of  freedom  robot  was  conducted  in  order  to 
determine  which  configuration  would  be  better  suited  for  the  A^RM  system.  As 
a  “first-look”  analysis,  the  foundation  was  set  to  expand  this  research  into  more 
complex  systems. 

Through  careful  selection  of  the  physical  configuration  and  mass  distribution 
characteristics  of  a  robotic  mainpulator,  greater  efficiency  and  minimal  control 
complexity  can  be  achieved.  Methods  of  achieving  minimal  control  complexity  has 
been  addressed  in  the  literature  (!].  These  methods  have  generally  been  applied  to 
manipulators  with  constant  or  zero  payloads.  For  the  general  manipulator  designed 
to  be  flexible  enough  to  handle  multiple  operations,  constant  payload  assumptions 
are  not  suitable.  Therefore,  the  effects  of  variable  payloads  were  considered  in  the 
analyisis. 

In  addition,  development  of  analytical  tools  to  measure  the  performance  of 
these  types  of  manipulators  has  not  been  presented  to  any  great  extent.  Shin-Min 
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Song  aiul  .Tong-Kil  Lee  in  their  paper  The  Mechanical  Efficiency  And  Kinemat¬ 
ics  Of  Pantograph  Type  Manipulators,  [14],  examine  using  mechanical  work  as  a 
measure  of  efficiency.  Song  and  Lee’s  work  looked  at  massless  systems  in  a  limited 
area  of  the  workspace.  Their  research  did  not  present  a  total  representation  of 
the  efficiency  of  the  manipulator  configurations  considered.  First,  Song  and  Lee’s 
conclusion  that  the  parallel  was  100%  efficient,  is  true  only  for  a  poriton  of  the 
workspace.  Second,  inertial  terms  and  the  mass  of  the  manipulators  in  the  gravita¬ 
tional  terms,  i.e.  a  massless  arm  was  assumed,  in  the  dynamic  equations  of  motion 
were  not  considered. 

1.1  Background 

The  comparison  of  different  type  of  robotic  manipulator  configurations  re¬ 
quired  a  somewhat  generic  tool  that  could  be  equally  applied  to  all  types  of  con¬ 
figurations  considered.  The  tool  applied  was  the  determination  of  mechanical  effi¬ 
ciency  in  the  workspace  of  a  robot  configuration.  This  efficiency  was  determined 
for  a  serial  and  a  parallel  kinematic  configuration.  The  effects  of  “tuning”  on  the 
efficiency  were  also  considered.  These  configurations  are  well  known  in  robotics 
and  are  defined  in  the  following  sections. 

1.1.1  Serial  Manipulator  The  serial  manipulator  is  an  open  kinematic  chain 
with  torque  producing  actuators  on  each  revolute  joint,  as  shown  in  Figure  1.1. 

These  robots  have  one  joint  and  one  corresponding  link  for  every  degree 
of  freedom.  This  is  the  most  widely  used  configuration  for  robotic  applications. 
Further  discussion  on  these  type  of  robots  can  be  found  in  [1],  [3],  and  [13]. 

1.1.2  Parallel  Manipulator  The  parallel  manipulator  configuration ,  as  shown 
in  Figure  1.2,  is  a  closed  link  kinematic  chain  robot  having  at  least  one  redundant 
degree  of  freedom  in  its  structure. 
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Figure  1.1.  Serial  Manipulator  Configuration 
Furhter  discussion  on  these  configurations  can  be  found  in  [1]  and  [13]. 

1.1.3  Tuning  Control  of  a  manipulators  position,  velocity,  and  acceleration 
is  a  very  complex  topic.  The  equations  of  motion  that  model  the  movement  of  the 
robot  are  non-linear,  coupled,  ordinary  differential  equations.  In  practice,  clzissical 
control  of  this  motion  is  often  inadequate,  [Ij.  A  large  amount  of  current  research 
is  directed  toward  using  modern  optimal  control  techniques  to  compensate  for 
these  inadequacies.  However,  with  careful  design  analysis  these  non-linearities 
and  coupling  effects  can  be  reduced  to  a  minimum.  Reducing  these  non-linear 
effects  is  called  “tuning”.  Complete  elimination  of  non-linear  and  coupling  effects 
allows  the  system  to  be  modelled  as  a  single  input  -  single  output  system  for  each 
actuator.  This  type  of  system  would  be  the  least  complicated  and  the  easiest 
to  control.  The  analysis  technique  for  reducing  nonlinear  and  coupling  terms  in 
the  equations  of  motion  was  developed  by  Haruhiko  Asada  and  Kama!  Youcef- 
Toumi  in  their  book  Direct-Drive  Robots:  Theory  and  Practice  [Ij.  Asada  and 
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Figure  1.2.  Parallel  Configuration 


Youcef-Toumi  were  able  to  design  a  manipulator  with  no  dependence  on  joint 
angle,  defined  as  “configuration  invariance”,  and  no  coupling  effects,  defined  as 
“dynamically  decoupled”.  This  resulted  in  equations  of  motion  that  were  linear, 
uncoupled  ordinary  second  order  differential  equations  with  constant  coefficients 
for  the  nominal  payload  for  which  the  manipulator  was  tuned.  When  the  payload 
varies  from  the  nominal  case,  all  the  non-linear  terms  of  the  equations  of  motion 
reappear.  The  dynamics  of  the  robotic  system  are  no  longer  linear  and  may  require 
model  based  control  techniques,  (8],  to  obtain  adequate  feedback  control. 

1.1.4  Gravity  Compensation.  In  addition  to  dynamic  decoupling  and  con¬ 
figuration  invariance,  the  gravitational  terms  of  the  equations  of  motion  can  be 
eliminated  for  some  nominal  load  [13].  The  “gravity  compensation”  constraints  are 
compatible  with  the  dynamic  decoupling  and  configuration  invariance  constraints 
and  can  also  be  included  in  the  term  “tuning”.  Therefore  all  the  constraints  can 
be  applied  to  the  design  of  the  manipulator  simultaneously. 
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1.].5  Definition  of  Mechanical  Efficiency.  According  to  Be«»r  and  Johnston, 
[2,  page  41  Ij,  mechanical  efficiency  is  defined  as  the  amount  of  work  out  of  a  system 
divided  by  the  the  amonut  of  work  put  into  the  system.  Work  is  defined  in  the 
plane  as, 


W  =  F  df 


(1.1) 


where  F  is  a  force  acting  through  a  displacement  dr.  When  a  moment  is  acting  on 
a  rigid  body  the  work  done  can  be  written  as, 

W  =  j^MQdt  (1.2) 


where  M  is  the  moment  and  dd  is  the  angular  displacement  in  radians. 


In  Song  and  Lee’s  andysis,  [14],  the  output  work  was  found  from  moving 
the  payload  parallel  to  the  gravity  vector  some  finite  distance.  Because  the  force 
involved  was  constant  the  work  was  easily  integrable  to  be  mg  Ay,  where  Ay  is  the 
displacement. 


1.2  Method  Of  Approach 

Using  Song  and  Lee’s  mechanical  work  as  a  measure  of  efficiency,  the  serial 
and  parallel  robotic  arm  configurations  were  evaluated.  The  equations  of  motion 
for  these  well-known  manipulators  were  readily  available,  [13],  making  these  con¬ 
figurations  well  suited  for  this  analysis.  Inertia  and  gravitational  terms  of  the 
equations  of  motion  were  included  in  the  analysis. Structural,  dimensional,  and 
motor  requirements  for  the  arm  design  was  not  a  part  of  this  analysis. 

A  basic  design  had  to  be  drawn  up  to  set  a  baseline  for  the  configurations. 
The  manipulator  is  being  designed  to  emulate  human  arm  motion.  The  dimensions 
of  the  configurations  were  assumed  to  be  similiar  to  those  of  the  50th  percentile  Air 
Force  male  [4].  Once  the  basic  design  was  determined,  link  mass  was  redistibuted 
in  order  to  construct  manipulator  configurations  that  were  gravity  balanced,  con- 
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figuration  invariant.  Efficiencies  of  each  configuration  were  then  evahiated  for  tlie 
following  cases: 

•  Only  gravity  terms  of  the  equations  of  motion. 

-  No  arm  mass  included. 

-  Arm  mass  included. 

•  Inertia  terms  only. 

-  No  configuration  invariance  tuning  applied. 

-  Configuration  invariance  tuning  applied. 

•  All  terms  of  the  equations  of  motion  included. 

-  Nominal  case  -  no  tuning. 

-  Gravity  compensated  case. 

-  Gravity  compensated  and  configuration  invariant  case. 

Redistribution  of  mass  to  decrease  the  effects  of  non-linear  terms  in  the  equations 
of  motion  is  often  refered  to  as  “tuning”.  The  tuning  is  dependant  on  a  particu¬ 
lar  payload.  The  cases  studied  here  assumed  that  the  manipulator  was  tuned  for 
zero  payload.  The  effect  of  non-zero  payload  on  efficiency  was  then  determined. 
Efficiency  was  found  in  the  first  quadrant  of  the  reachable  workspace  after  de¬ 
termining  that  the  other  quadrants  were  symmetrically  similiar  to  quadrant  one. 
To  include  inertia  terms,  the  trajectory  was  assumed  to  be  in  a  straight  fine  at 
constant  acceleration. 


l.S  Summart 


In  every  case,  the  parallel  manipulator  was  more  efficient  than  the  serial.  When 
gravity  balancing  was  imposed,  efficiency  of  both  the  serial  and  parallel  configu- 
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rations  increased.  Also,  configuration  invariance  tuning  made  the  parallel  robot 
more  efficient. 

This  document  is  organized  in  the  following  manner.  In  chapter  II,  the 
human  arm  characteristics  are  defined  along  with  how  these  characteristics  are 
applied  to  the  robot  arms  and  how  the  tuned  cases  are  achieved.  Then,  chapter  III 
presents  the  analysis  of  the  mechanical  efficiency.  Finally,  chapter  IV  considers  the 
conclusion  and  recommendations.  In  the  appendices  A  and  B,  the  actual  computer 
programs  developed  are  included. 
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II.  Anthropomorphic  Manipulator  Design 


The  choice  to  design  a  manipulator  to  emulate  the  motion  of  a  human  arm 
was  made  to  be  compatible  with  future  research  efforts  at  the  Air  Force  Institute  of 
Technology  (AFIT)  and  Armstrong  Aerospace  Medical  Research  Lab  (AAMRL). 
One  apphcation  for  using  an  anthropomorphic  robotic  manipulator  is  as  a  surrogate 
in  a  telepresence  application.  Making  the  robot  arm  anthropomorphic  makes  the 
telepresence  task  more  intiutive  to  the  human  operator.  In  this  application  the 
human  operator  is  removed  from  a  hostile/hazardous  environment  and  operates 
the  robot  by  strapping  on  a  sensored  exoskeleton.  Movement  of  the  the  exoskeleton 
results  in  the  robot  executing  the  same  movement. 

Specifically,  the  design  will  emulate  the  motion  of  a  50th  percentile  Air  Force 
male  which  was  documented  by  H.  T.  E.  Hertzberg  at  AAMRL  [4].  The  intended 
robot  design  will  have  three  degrees  of  freedom,  all  revolute,  including  two  orthog¬ 
onal  degrees  of  freedom  at  the  shoulder  and  one  degree  of  freedom  at  the  elbow. 
The  elbow  axis  of  rotation  and  one  shoulder  axis  of  rotation  are  parallel.  Using 
these  two  rotations  as  independent  coordinates,  any  reachable  position  in  a  plane 
can  be  described  by  these  coordinates.  With  these  two  degrees  of  freedom,  any 
plane  in  the  robot’s  work  volume  can  be  represented,  alleviating  the  neccessity  of 
having  to  look  at  the  three  degree  of  freedom  equations  of  motion  for  this  first-look 
analysis.  When  gravitational  terms  are  omitted  from  the  equations  of  motion,  a 
plane  perpendicular  to  gravity  is  simulated.  When  gravity  is  included,  a  plane 
parallel  to  gravity  is  simulated. 

This  chapter  will  address  the  design  requirements  for  human  arm  emulation 
specifically  in  terms  of  dimension,  range  of  motion,  speeds,  and  accelerations.  From 
these  specifications,  the  dimensions,  weights,  kinematics,  and  dynamic  equations 
of  motion  for  both  a  serial  and  paraUel  two  degree  of  freedom  anthropomorphic 
configuration  are  characterized. 
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g.  1  Human -like  Sperificaticns 

To  perforin  as  a  human  surrogate,  the  manipulator  must  be  capable  of  repro¬ 
ducing  the  movements  and  load  carrying  capacity  of  an  average  human  arm.  And 
the  manipulator  must  as  a  minimum  be  able  to  reach  what  the  average  arm  can. 
The  intent  of  this  manipulator  design  is  to  emulate  the  motion  of  a  50th  percentile 
Air  Force  male.  These  specifications,  except  for  velocity  and  acceleration,  were 
developed  by  H.  T.  E.  Hertzberg  at  the  Armstrong  Aerospace  Medical  Research 
Laboratory  [4].  The  dimensions  of  this  average  male  arm  are  summarized  in  Table 
2.1  [4,  page  499]. 


Arm  Section 

Length 

(inches) 

Shoulder  to  Elbow 

14.3 

Elbow  to  Handtip 

18.9 

Wrist  to  Handtip 

7.5 

Table  2.1.  Average  Human  Arm  Dimensions,  [4] 

The  range  of  motion  specifications  are  in  Table  2.2  [4,  page  545). 


Motion 

Range 

(degrees) 

Shoulder  Bend 

249 

Shoulder  Twist 

182 

Elbow  Bend 

142 

Table  2.2.  Average  Human  Range  of  Motion,  [4] 

These  specifications  of  the  the  range  of  motion  for  the  average  human  arm 
define  the  human  workspace. The  average  human  arm  velocities  and  accelerations 
are  found  in  Eugene  I.  Rivin’s  book  Mechanical  Design  Of  Robots  [13,  page  10). 
The  experimental  data  for  these  specifications  is  found  in  B.  A.  Petrov’s  work  [12], 
but  unfortunately  this  has  never  been  translated  from  Russian  to  English.  Human 
arm  speed  and  acceleration  are  summarized  in  Table  2.3. 
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Motion 

Maximum  angular 
speed  (rad/s) 

Maximum  angular 
acceleration  (rad/s*) 

Shoulder  Bend 

7.0 

70 

Shoulder  Twist 

10.0 

120 

Elbow  Bend 

17.0 

300 

Table  2.3.  Average  Human  Arm  Speed  and  Acceleration,  [13,12] 

The  motions  described  in  these  Tables  2.2,  and  2.3  refer  to  shoulder  bend, 
shoulder  twist,  and  elbow  bend.  These  motions  are  defined  as  follows: 

Shoulder  Bend  The  angle  the  shoulder  joint  moves  through  in  a  plane  parallel 
to  the  body  sagital  plane. 

Shoulder  Twist  The  angle  through  which  the  shoulder  joint  rotates  about  a 
vertical  axis,  i.e.,  across  the  chest. 

Elbow  Bend  The  angular  motion  of  the  elbow  joint.  Not  the  twisting  motion  of 
the  forearm. 

i.i  Configurations 

From  the  human  arm  specifications,  the  basic  design  of  the  serial  and  parallel 
configuration  can  be  determined.  Inherent  to  each  configuration  is  a  link  that  goes 
from  the  shoulder  to  the  elbow,  which  will  be  called  length  /i,  and  a  forearm,  which 
will  be  called  length  /j  for  the  serial  arm,  as  labeled  in  Figure  1.1,  and  length 
for  the  parallel  as  labeled  in  Figure  1.2. 

To  compare  the  configurations,  it  was  necessary  to  postulate  a  design  for 
each  configuration  of  manipulator  to  fix  parameters  of  length,  mass,  centers  of 
gravity,  and  moments  of  inertia.  For  the  sake  of  simplicity,  the  cross  sections 
of  the  manipulators,  as  shown  in  Figure  2.1, were  assumed  to  be  rectangular  and 
hollow,  and  are  the  same  size  for  all  links.  This  cross  section  is  based  on  human 
arm  dimensions  from  [4]. 
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Figure  2.1.  Rectangular  Cross  Section  of  Each  Link 


The  m-'.terial  used  to  construct  the  links  was  assumed  to  be  readily  obtainable 
aluminum  with  density  a;  2.8  x  10®  An  aluminum  with  this  density  could 
reasonably  be  selected  as  an  inexpensive  material  to  manufacture  the  arm  links. 
From  this  density  and  volume  of  the  links,  the  mass  of  each  link  is  obtained,  i.e., 
mass  =  p  X  cross  sectional  area  x  length. 

Based  on  the  human  arm  dimensions.  Table  2.1,  link  1  is  then  set  equal  to 
0.36322  meters,  and  link  2  is  0.6705  meters  in  length.  From  these  lengths  the  mass 
of  link  1  was  then  calculated  to  be  equal  to  0.6667  kg,  and  link  2’s  mass  equals 
1.3333  kg,  twice  that  of  link  1. 

All  motors  were  assumed  to  be  of  6  kg.  A  review  of  current  motor  specifi¬ 
cations  showed  that  the  Moog  model  304-008  brushless  D.  C.  motor  was  capable 
of  achieving  near  human  arm  sppeds,  see  Table  2.3  of  more  than  10  radians/sec. 
The  motor  is  probably  slightly  undersized  for  the  task  at  joint  one,  but  is  within 
practical  approximation  limits. 
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As  a  reasonable  estimate  of  the  payload,  it  was  assumed  that  the  mass  of  the 
payload  was  equal  to  l/5th  of  the  serial  arm  mass,  where  the  serial  arm  mass  was 
made  up  ot  the  mass  of  link  1  and  2,  and  the  mass  of  the  motor  at  joint  2,  as  shown 
in  Figure  1.1.  The  resulting  payload  was  1.6  kg.  Structural  integrity  was  not  at 
issue  for  the  development  of  the  basis  for  evaluation  of  mechanical  efficiencies  of 
manipulator  configurations. 

In  order  to  present  a  more  comprehensive  comparison,  the  variables  chosen 
to  be  independent  generalized  coordinates  will  be  the  joint  angles  of  the  serial 
manipulator,  i.e.  Oi,  and  $2i  see  Figure  1.1.  These  angles  are  consistently  used  for 
both  configurations. 

From  this  average  human  arm  data,  the  weights,  and  dimensions  of  each 
configuration  can  be  determined  to  within  a  reasonable  proximity.  Design  specifics 
can  now  be  discussed. 

S.i.l  Serial  Configuration.  Most  currently  available  robotic  manipulators 
have  serial  open  loop  chain  kinematics.  Serial  manipulators  are  such  that  each  link 
is  attached  to  the  end  of  the  previous  link  allowing  for: 

•  More  reach 

•  Movement  in  another  degree  of  freedom 

•  More  positioning  flexibility,  multiple  paths  to  same  cartesian  position,  (i.e. 

redundant  degrees  of  freedom). 

The  motors  or  actuators  for  serial  manipulators  can  be  located  at  the  joint  of  the 
the  link  driving  the  link  as  in  the  case  of  “direct-drive”  robots,  remotely  located 
through  some  kind  of  transmission  mechanism  such  as  gears  and  shafts.  Direct- 
drive  robots  are  particulary  well-suited  for  high  speed  applications.  Human  arm 
emulation  requires  high  joint  speeds.  Therefore,  the  A^RM  prototypes’  drive  sys- 
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terns  were  assumed  to  with  direct-drive  motors.  For  the  serial  configuration  the 
motor  driving  link  2  was  assumed  to  be  located  at  joint  2. 

The  following  notation  is  used  to  describe  the  parameters  of  the  serial  ma¬ 
nipulator: 

nta:  The  mass  of  the  payload.  (Nominally  =  1.6  kg) 
mi:  The  mass  of  link  1.  (=  0.6667  kg) 
li".  The  length  of  link  1.  (=  0.36322  meters) 
m2:  The  mass  of  Unk  2.  (=  1.3333  kg) 

I2:  The  length  of  hnk  2.  (=  0.6705  meters) 
m^:  The  mass  of  the  motor  at  joint  2.  (=  6.0  kg) 

This  notation  will  be  used  throughout  this  text  unless  otherwise  indicated.  The 
centers  of  gravity  of  each  link  will  additionally  subscripted  with  the  letter  c. 

Parallel  Configuration.  The  parallel  manipulator  configuration  arises 
from  using  a  mechanical  linkage  to  transmit  the  torque  of  a  motor  physically  located 
off  the  joint  axis  of  the  joint  of  a  particular  link.  In  addition,  the  configuration  is 
called  parallel  because  the  linkage  is  constructed  such  that  it  connects  to  form  a 
parallelogram.  A  parallelogram  is  chosen  to  simphfy  the  relationship  of  the  angles 
of  the  links.  This  method  of  transmission  places  the  motor  weight  (which  generally 
is  quite  significant)  closer  to  the  base  of  the  robot  arm,  relieving  the  arm  of  the 
additional  torque  caused  by  the  motion  of  the  motor. 

In  this  analysis,  the  transmission  Unkage  is  used  to  locate  the  motor  driving 
joint  2  at  joint  1.  Two  links  are  added  to  the  serial  configuration  to  construct  the 
parallel.  In  order  to  maintain  the  reach  of  the  average  human  arm,  the  length  of 
the  link  extending  from  the  end  of  link  1  to  the  payload,  is  unchanged.  This  link 
has  been  designated  as  link  4,  but  is  indeed  equivalent  in  length  to  link  2  of  the 
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serial  manipulator.  The  transmission  mechanism  then  makes  up  the  parallelogram. 
The  side  extending  back  from  the  elbow  is  assumed  to  be  l/4th  of  the  length  of 
link  4,  which  equals  the  length  of  link  2.  This  number  was  chosen  as  a  reasonable 
compromise  between  the  values  used  in  the  Asada  parallel  mechanism  ,[1],  and 
the  Kazerooni  parallel  direct  drive  robot,  (6j.  The  other  additional  link,  link  3,  is 
naturally  the  same  length  as  hnk  1  to  complete  the  parallelogram. 

The  notation  used  for  the  serial  manipulator  is  extended  to  the  parallel  ma¬ 
nipulator  configuration.  When  necessary  to  distinguish  between  the  two,  an  ad- 
dtional  subscript  p  will  be  used  for  the  parallel  case  and  an  s  for  the  serial. 

2.3  Forward  Kinematics 

The  forward  kinematic  equations  are  used  to  relate  a  position  in  a  base  co¬ 
ordinate  frame  to  the  rotating  and  translating  body  axis  coordinate  of  the  payload 
or  end  effector  of  the  robot  arm.  For  the  configurations  tested,  two  rotational 
joints  in  a  plane  are  considered.  The  kinematic  equations  are  equivalent  for  each 
configuration. 

The  kinematic  equations  are  as  foUows: 

T  =  licos{6i)  +  l2Cos{9i $2)  (2.1) 

y  =  -f- Z2sm(^i -I- ^2)  (2.2) 

Knowing  the  joint  angles  of  the  robot  arm  the  position  of  the  end  effector 
is  expressed  in  the  based  coordinate  frame  coordinates  (x,y).  These  equations 
describe  the  points  of  the  x-y  plane  that  are  reachable  by  the  manipulator.  This 
area  of  the  plane  is  commonly  csdled  the  robot’s  “workspace”. 

2.4  Inverse  Kinematics 

Typically,  the  position  of  the  end  effector  is  known  in  base  coordinates  and 
the  joint  angles  are  desired.  The  kinematics  equations  that  solve  for  the  joint 
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angles  given  the  base  coordinate  values  are  called  the  inverse  kinematic  equations. 
For  a  two  dimensional  problem  this  is  easily  done  in  closed  form,  [13,  page  42],  [3, 
chapter  3].  For  this  problem  the  equations  are  given  in  Rivin’s  book,  [13,  page  44], 


as  follows; 


.  +  „>)-((> +  /|)\ 

V  2l^l,  I 

,  / -(l2Stn^2)ar  -|-  (/i  +  l2cos02)y\ 
\  {l2sin62)y  +  (/,  +  / 


(2.3) 

(2.4) 


Because  the  inverse  tangent  function  is  not  well  behaved  for  360  degrees  of 
revolution,  it  is  more  convenient  to  use  a  relationship  that  looks  at  the  signs  of  both 
X  and  y.  In  the  Fortran  programming  language  this  function  is  known  as  ATAN2. 
It  requires  an  input  of  two  arguments,  x  and  y.  If  the  point  in  question  is  in 
quadrants  1  and  4  of  the  x-y  plane,  then  Equation  2.4  is  sufficient.  For  quadrant  3, 
equals  the  result  of  Equation  2.4  minus  180  degrees.  In  quadrant  2,  180  degrees 
must  be  added  to  the  result  of  Equation2.4  to  find  0i.  Using  these  relationships 
the  joint  angles  are  obtainable  from  any  portion  of  the  manipulators  workspace. 


S.5  Dynamic  Fquations  of  Motion. 

The  instantaneous  torques  required  for  each  actuator  at  a  point  in  the  work 
space  are  found  through  rigid  body  dynamics.  These  relationships  are  the  equations 
of  motion  representing  the  manipulators.  Robot  arm  dynamic  equations  depend 
on  the  joint  angles,  the  velocity,  and  the  acceleration  of  those  angles.  In  general, 
ignoring  drive  system  dynamics,  these  equations  are  of  the  form: 

f  =  Di''+  h  +  g  (2.5) 

where  D  is  the  inertial  tensor,  5  is  a  vector  of  corioUs  and  centrifugal  terms,  and 
^  is  a  vector  of  gravitational  terms. 

A  MACSYMA,  [15]  program  was  developed  to  generate  the  symbohc  equa¬ 
tions  of  motion  for  both  serial  manipulators  and  closed  link  kinematic  chain  ma- 


r 
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nipiilaiors.  The  code  and  its  development,  implementation,  and  limitations  are 
included  in  appendix  A. 

The  equations  for  each  configuration  follow 

1.5.1  Serial  Configuration.  Based  on  terms  (lengths,  masses,  and  moments 
of  inerita)  defined  in  Figure  1.1,  the  equations  of  motion  of  the  serial  manipulator 
are  as  follows: 

Tj  =  4-  /i  -I-  (m2  +  m„,)/j  +  ^2^lc  d" 

+mo/i  +  mJl  +  2/i(m2/2c  +  m<,/2)coj^2]^i 

+  [m2/2c  4-/2  +  m^/j  -I-  I\{m2l2c  +  mo/2)coj^2]^'2 

—  2/1(012/20  +  mo/2)^i^2^/n.^2  ~  +  rnol2)v\sin92 

4-(”Ji^o  +  m2/i  +  rrioli)  g  cos$i  +  {rriilic  +  mo/2)  9  co3(0i  +  ^2)  (2.6) 

Ti  =  +  I2  +  mJl  +  Ii{m2l2c  +  TTioh)  cos92]0i 

+  (’Ti2^L  +  h  +  ^ol\)9t  +  (”12^20  +  mol2)9\  sin92 

+(^2/20  +  mo/2)  9  cos{9i  +  ^2)  (2.7) 

2.5.2  Parallel  Configuration.  Based  on  terms  (lengths,  masses,  and  mo¬ 
ments  of  inerita)  defined  in  Figure  1.2,  the  equations  of  motion  of  the  parallel 
manipulator  are  as  follows:  (The  generalized  coordinates  used  in  these  equations 
are  the  same  common  independant  coordinates  used  in  the  serial  manipulator.) 

Ti  =  (mi/jp^  +  /i  +  rnsll^  -f  /s  +  rn^ll  -|-  mJl 
-(^3/2/30  -  rUiliUc  -  mo/1/4)  co392]9i 
-[(m3/2/3c  -  rritliU^  -  mo/1/4)  cos^2]^2 
+  (m3/2/3c  -  rn^liUc  -  rriolxU)  sin929\ 

4-(^i/ic  +  m^lz^  -I-  m^li  -1-  mo/i )  g  cos9i  (2.8) 

T2  =  [m2/2p.,  4-/24-  m3/2  4-  rtitll^  +  I*  +  mJl  +  I„ 

-(m3/2/3e  -  m*hUc  -  mo/1/4)  cos92]9i 
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+  [^2^2pc  +  ^2  +  ^3^lp  +  ”^4^^  +  A  +  TTIo/J  +  /o]^2 

-{mshhc  ~  ttiaIiUc  -  mJiU)  sinff2^] 

-i'^2kc  +  rnsU  -  THiUc  -  2720/4)  9  C03{9i  +  62)  (2.9) 


2.6  Achievement  of  Minimum  Control  Complexity 

As  stated  earlier,  the  analysis  of  these  manipulators  included  enhancing  the 
efficiency  by  minimizing  control  complexity.  The  efficiency  enhancement  techniques 
investigated  were: 

•  Dynamic  Decoupling 

•  Configuration  Invariance 

•  Gravity  Compensation 

These  techniques  are  compatible,  i.e.  they  can  be  implemented  simultaneously 
with  the  exception  that  the  serial  configuration  can’t  be  dynamically  decoupled. 
However,  unless  the  configuration  of  the  manipulator  can  change  dynamically  with 
payload,  these  conditions  can  only  be  met  for  some  nominal  payload.  This  research 
effort  is  the  first  time  that  the  effects  of  a  varying  payload,  other  than  the  nominal 
“tuned”  payload,  have  been  examined.  This  section  will  address  the  theory  behind 
minimum  control  complexity,  and  how  it  was  applied  to  the  seri2d  and  parallel 
manipulators. 

2. 6. 1  Design  for  Dynamic  Decoupling  and  Configuration  Invariance  Asada 
and  Youcef-Toumi  [1,  chapter  4]  presented  a  method  for  reducing  the  control  com¬ 
plexity  of  a  manipulator  by  simplifying  the  dynamic  equations  of  motion.  These 
equations  are  in  general  highly  coupled,  non-hnear,  second  order  ordinary  differen¬ 
tial  equations.  Asada  and  Youcef-Toumi  have  examined  how  to  design  a  manipu¬ 
lator’s  mechanical  configuration  to  achieve  decoupled  dynamics  and  configuration 
invariance.  The  dynamic  decoupling  eliminates  the  off-diagonal  terms  of  the  inertia 
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tensor  and  simultaneously  eliminates  the  coriolis  and  centrifugal  terms  resulting 
in  a  single  input  -  single  output  control  idgorithm  for  each  actuator.  The  config¬ 
uration  invariance  reduces  the  non-linear  equations  to  linear,  ordinary  differentiiil 
equations  with  constant  coefficients. 


Dynamic  Decoupling.  Ignoring  the  motor  dynamics,  the  equa¬ 
tions  of  motion  for  a  manipulator  with  revolute  joints  can  be  expressed  as  [1]: 


Ti  = 


+  r, 


»« 


(2.10) 


where  Dij  is  the  i-j  element  of  the  inertia  matrix  and  Tg{  is  the  gravity  term.  For 
dynamic  decoupling  the  off  diagonsJ  elements  of  the  inertia  matrix  are  set  to  zero  by 
careful  configuration  design.  These  constraints  are  different  for  each  configuration 
and  are  presented  in  full  detail  after  further  deveopment.  With  the  off  diagonal 
terms  equal  to  zero,  the  equations  of  motion  reduce  to: 


Eliminating  the  off  diagonal  inertia  terms  has  significantly  reduced  the  complexity 
of  the  equations  of  motion.  However,  there  are  still  non-linear  terms  arising  due 
to  the  dependence  of  the  manipulator  on  joint  orientation. 


Configuraiion  Invariance.  Another  simplification  that  can  be  done 
to  the  dynamic  equations  of  motion  is  to  make  the  manipulator  independent  of  spa¬ 
tial  orientation.  This  simplification  eliminates  the  non-linear  terms  of  the  equations 
of  motion.  Equation  2.10,  except  for  the  gravity  terms.  This  implies  from  Equation 
2.10  that  the  following  relationship  must  be  true: 


Substituting  Equation  2.12  into  Equation  2.10  the  equations  of  motion  become: 


r*  =  Dji  'EdJj  +  Tgi  (2.13) 

I 
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Except  for  the  gravity  terms,  these  equations  are  linear  with  constant  coefficients. 
Combining  the  configuration  invariance  with  the  decoupling  completes  the  simpli¬ 
fication  process  yielding  linear  (except  for  gravity  terms),  decoupled  equations  of 
motion. 

2.6.2  Design  for  Gravity  Compensation  In  addition  the  reduction  of  non- 
liearity  obtained  from  configuration  invariance  and  dynamic  decoupling,  the  non¬ 
linear  gravitational  terms  in  the  equations  of  motion  can  be  completely  eliminated. 
The  basic  idea  is  to  balance  the  manipulator  about  each  actuated  pivotal  point. 
This  is  usually  done  by  using  the  weight  of  the  actuator  as  a  counterbalance  to 
the  weight  of  the  link  (or  set  of  links)  it  is  driving.  In  order  to  do  this  coun¬ 
terbalancing,  the  link  has  to  be  extended  beyond  its  joint  of  rotation.  This  may 
physically  impede  the  motion  of  the  manipulator,  limiting  its  workspace  unless 
carefully  designed  to  avoid  this  problem. 

2.6.S  Applied  Minimal  Control  In  order  to  conduct  a  fair  comparison  of 
both  manipulators,  parameters  of  length  and  mass  must  established.  The  following 
conditions  were  used  to  establish  a  baseline  configuration: 

•  The  human  arm  dimensions  were  imposed  on  each  manipulator. 

•  For  the  parallel  manipulator,  the  added  length,  /?,  was  arbitrarily  chosen  to 
be  I2  =  I4/2  for  all  cases  of  the  analysis. 

•  Any  added  linkage  that  wsis  required  to  implement  tuning  was  added  at  a 
constant  mass/unit  length. 

•  The  weight  of  the  motors  was  used  for  balancing  when  applicable. 

•  The  nominal  payload  used  for  tuning  was  =  0. 

2.6.4  Serial  Manipulator.  The  tuning  conditions  were  applied  to  the  serial 
manipulator.  Looking  at  the  equations  of  motion,  Equation  2.7,  it  can  be  deter- 
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mined  that  only  configuration  invariance  and  gravity  compensation  are  achieveable. 
The  ofT-diagon2d  terms  of  the  inertia  tensor  are  all  always  positive  and  hence  cannot 
be  eliminated.  Therefore,  two  tuning  conditions  have  to  be  met  (from  Equation 
2.7). 

For  configuration  invariance  the  following  condition  must  hold: 

=  0  (2.14) 

This  is  adso  the  condition  for  link  2  to  be  gravity  compensated.  For  the  nominal 
baseline,  however,  the  payload  is  zero.  Therefore,  the  condition  for  configura¬ 
tion  invariance  reduces  to; 

=  0  (2.15) 


For  gravity  compensation  the  condition  is: 

rtiilu  -f  mill  d-  moll  =  0  (2.16) 


With  the  all  the  lengths  and  masses  fixed  in  the  basehne  and  with  ruo  =  0,  the 
only  variable  then  is  1^.  Solving  for  lu  the  condition  becomes: 


lu 


mi 


(2.17) 


Note  that  without  the  configuration  invariance  condition  imposed,  gravity  com¬ 
pensation  is  not  achievable  for  the  serial  manipulator. 


To  enforce  the  configuration  invariance  and  gravity  compensation,  the  loca¬ 
tions  of  the  motors  were  moved  to  act  as  a  mass  counterbalance.  For  the  serial 
manipulator,  the  physical  configuration  changed  to  appear  as  in  Figure  2.2. 


In  Figure  2.2,  the  two  new  lengths  introduced,  and  /j  are  the  lengths  the 
hnks  had  to  be  extended  to  put  the  motors  in  the  location  necessary  for  tuning. 
These  lengths  change  the  tuning  conditions  to  be; 


TTiilu  +  m2li  +  mJi  -  ~  rrimli  =  0 


(2.18) 
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and 

mj/ie  +  mj/i  +  mo/i  -  -  mj/jc  =  0  (2.19) 

The  addtional  mass  rn^  and  m,  is  added  assuming  constant  cross  sectional  area 
and  constant  density  of  the  materisd.  Therefore,  these  masses  can  be  expressed 
mathematically  in  terms  of  the  additional  length  added.  Therefore  Equations  2.18 
and  2.19  are  a  system  of  two  equations  and  two  unknowns,  l\  and  /j.  Solving  the 
equations,  the  lengths  are; 

•  /j  =  0.4483m€t€rs 

•  =  0.1455mctcrs 

This  modified  configuration  was  used  in  the  efficiency  calculations  for  the  plots  in 
Chapter  III. 

t.6.5  Parallel  Manipulator.  The  tuning  conditions  for  the  parallel  manip¬ 
ulator  are  obtsdned  from  Equation  2.9.  The  parallel  manipulator  can  be  made 
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rompletely  decoupled,  invariant,  and  gravity  compensated.  To  achieve  this  tuning, 
three  conditions  must  be  met  to  balance  the  centers  of  gravity  of  the  links.  These 
conditions  are  as  follows: 

For  configuration  invariance  and  dynamic  decoupling, 

m3l2l3<^  -  f'UhUc  -  =  0  (2.20) 

For  gravity  compensation, 

milic  +  mslzc +  TUoli  =  0  (2.21) 

+  maZj  —  ^14/4^  —  rrioU  =  0  (2.22) 

For  the  parallel  manipulator,  configuration  invariance  can  be  achieved  independent 
of  gravity  compciisation  and  vice  versa.  The  physical  configuration  of  the  parallel 
arm  was  changed,  consistent  with  the  way  the  serial  was  changed,  by  using  the 
location  of  the  motors  to  achieve  the  conditions  necessary  for  configuration  invari¬ 
ance  and  gravity  compensation.  The  physical  appearance  of  this  configuration  is 
shown  in  Figure  2.3. 

The  two  new  lengths  defined  in  the  physical  alteration  process  for  the  parallel 
arm  were,  l[  and  /j.  Where  l[  is  additional  link  length  added  for  tuning,  and  Zj 
designates  the  new  location  of  the  motor  driving  link  2.  These  new  parameters 
change  the  tuning  constraint  equations  for  gravity  compensation  of  the  parallel 
manipulator.  These  equations  are: 

miZie  +  msZst -f  m4Zx -f  m^Zi  -  Tn„Zi  -  m  jZj^  =  0  (2.23) 

mjZje  -I-  msli  -  m^l4c  -  rUoU  -1-  m^Zj  =  0  (2.24) 

Substituting  for  tttj  in  terms  of  length  l[,  the  equations  were  solved  for  Zj  and  Z2.  For 
the  case  of  imposing  gravity  compensation  only  without  configuration  invariance, 
Z4,  was  assumed  to  be  Z4/2.  Applying  the  gravity  compensation  constraints  yielded: 
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Figure  2.3.  Physical  Configuration  of  Tuned  Parallel  Configuration 

•  =  0.0895  meters 

•  =  0.0466  meters 

When  imposing  the  configuration  invariance  constraint  along  with  the  gravity  com¬ 
pensation  constraints,  the  center  of  gravity  of  link  4  had  to  changed.  Through 
redistribution  of  the  mass  of  link  4,  the  new  center  of  gravity  was  driven  to 
lie  =  0.0629  meters  for  configuration  invariance.  The  new  locations  of  the 

motors  to  achieve  gravity  compensation  became: 

•  /j  =  0.0895  meters 

•  /j  =  0.0419  meters 

This  modified  configuration  was  used  in  the  efficiency  calculations  for  the  plots  in 
Chapter  III. 
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2. 7  Mechanical  Efficiency  Comparison 

Although  there  has  been  much  attention  given  to  how  to  build  and  tune 
manipulators  to  achieve  configuration  invariance,  dynamic  decoupling  and  gravity 
compensation,  very  little  attention  has  been  given  to  evaluating  and  comparing 
the  performance  of  these  manipulators.  Most  of  the  literature  [1],  [13],  addresses 
how  to  tune  for  a  constant  payload,  and  claims  that  payload,  indeed,  doesn’t  vary 
appreciably.  However,  when  considering  the  most  general  manipulator,  where  the 
application  or  task  of  that  manipulator  is  not  well  defined,  the  effects  of  a  varying 
payload  on  performance  are  of  great  concern.  Song  and  Lee,  [14],  present  a  method 
for  evaluating  the  performance  of  manipulators  using  mechanical  efficiency.  Song 
and  Lee,  [14],  considered  only  massless  systems  with  point  mass  payload.  The 
research  presented  here  expands  on  Song  and  Lee’s  work  breaking  new  ground  to 
include  inertial  and  gravitational  terms.  Coriolis  and  centrifugal  effects  have  been 
neglected  assuming  they  are  negligible  at  low  speeds. 

2.8  Summary 

This  chapter  has  addressed  the  design  considerations  of  the  parallel  and  se¬ 
rial  configurations.  The  first  consideration  is  the  design  goal  of  trying  to  define 
and  emulate  human  arm  motion.  Then  the  physical  layout  of  each  configuration 
was  examined.  Finally,  the  choices  made  to  apply  mechanical  efficiency  analysis 
to  each  configuration  in  a  nominal  and  various  partially  tuned  and  fully  tuned 
configurations  were  stated. 
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III.  Efficiency  Analysis 

As  a  starting  point,  Song  and  Lee’s  work,  [14],  was  recreated.  In  the  analysis, 
the  equations  of  motion  were  specialized  for  each  configuration  to  a  massless  arm 
moving  the  same  payload.  A  vertical  trajectory  was  assumed,  with  constant  x  and 
finite  y  displacement  in  cartesian  coordinates,  the  inverse  kinematic  equations, 
Equations  2.3  and  2.4,  were  solved  for  the  angular  displacement  and  substituted 
into  the  work  integral.  For  this  specialized  case  the  work  can  be  found  in  a  closed 
form  solution. 

Having  reproduced  Song  and  Lee’s  work,  see  section  3.2  for  detailed  anal¬ 
ysis,  the  next  step  was  to  add  in  the  mass  of  the  manipulators  and  look  at  the 
performance.  The  payload  was  still  being  displaced  by  the  same  amount,  but  the 
manipulators  were  moving  their  own  mass  in  addition  to  the  payload.  A  new  per¬ 
formance  parameter  had  to  be  introduced.  Both  manipulators  were  moving  the 
same  payload  by  the  same  amount.  The  comparison  of  performance  was  how  much 
total  work  had  to  be  done  by  the  manipulators  to  produce  that  movement.  The 
parameter  77  was  introduced  as  a  comparison  of  that  work,  where  y  is  defined  as, 

.  Input  Work  of  Parallel  Manipulator 

TJ  =  — - - - - - -  (3.1) 

Input  Work  of  Serial  Manipulator  ’  ' 

This  parameter  can  be  greater  than  one,  unlike  what  we  are  used  to  seeing  in 
efficiency.  When  y  is  greater  than  one,  the  serial  manipulator  has  done  less  total 
work  to  move  the  payload  a  prescribed  distance  than  the  parallel  manipulator. 
This  would  imply  that  the  serial  manipulator  is  more  efficient  for  that  movement. 
If  T]  is  less  than  1,  the  parallel  manipulator  is  more  efficient.  This  performance 
parameter  will  be  used  for  the  remainder  of  the  analysis. 

Even  with  the  mass  of  the  manipulator  added  in,  the  gravitational  terms  of 
the  work  integral  expression  were  still  integrable  in  a  closed  form.  A  closed  form 
solution  of  the  work  integral  was  not  possible  with  the  inertial  terms  included. 
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A  time  varying  trajectory  was  assumed  in  order  to  find  joint  position,  velocity, 
and  acceleration  for  the  constant  x,  0.1  meter  change  in  y  displacement.  Constant 
acceleration,  a,  in  the  y  direction  was  assumed  to  yield  the  following  trajectory 
profile: 


a 

(3.2) 

at 

(3.3) 

(3.4) 

(3.5) 

Through  the  manipulator  inverse  kinematics.  Equations  2.3  and  2.4,  expressions 
for  joint  velocities  and  accelerations  were  found. 


02 


^  _  liainjSi  +  02)y 

^  Iil2sin02 

^  _  [-IxsinOj  -  l2sin{0i  +  ^2)]  y 

*  I\l2sin82 

+  Iico40i0f  +•  +  ^2)^1  d"  l2Cos(9\  +  ^2)(^i  d"  ^2)* 

—l2sin{0i  +  02 


(3.6) 


(3.7) 


(3.8) 


01  —  {yd-  l2^in(0i  -h  02)(0J  -f  ^2)*  d-  [liCOS0x0f  -h  l2COs(0i  -f-  02){0l  d"  ^2)*] 

xcot{0i  d"  ^2)  d" 

-i-{/iCos^i  -f  l2cos{0i  +  02)  -  cot{0i  -I-  02)llisin0i  +  l2»in(0x  +  ^2)]}(3-9) 


Because  of  the  inability  to  integrate  these  equations  into  a  closed  form  solution 
of  mechanical  efficiency,  numerical  integration  was  done  when  the  inertial  terms 
were  included.  The  numerical  integration  was  based  upon  a  standard  trapezoidal 
rule,  (5,  page  777),  whose  accuracy  was  confirmed  by  comparing  the  numerically 
integrated  gravitational  term  results  to  those  of  the  closed  form  solution.  The 
integration  confirmed  accuracy  to  three  significant  figures.  The  detailed  MatrixX 
,  [16],  program  developed  for  this  numerical  analysis  is  included  in  appendix  B. 
The  expressions  for  (?i,  02,  0\,  and  02  and  the  equations  of  motion  of  the  serial 
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and  parallel  manipulators  were  evaluated  at  every  step  (twenty  steps  were  used  to 
achieve  three  significant  figure  accuracy)  in  the  integration.  The  efficiency  ratio 
was  determined  for  over  750  points  in  the  workspace.  With  these  large  expressions 
being  calculated,  and  considering  the  intense  number  of  iterations  required,  the 
numerical  process  of  evaluating  efficiency  when  including  inertial  terms  became 
very  time  consuming. 

Once  the  method  of  numerically  integrating  the  work  integral  expressions  was 
checked  out  for  the  analysis,  the  efficiencies,  t/',  were  computed  for  the  following 
cases  of  manipulator  dynamics  represented  by: 

•  Only  including  gravitational  terms, 

-  Massless  configurations, 

-  All  mass  included  configurations, 

•  Inertia  terms  only, 

-  Without  configuration  invariance, 

-  With  configuration  invariance, 

•  Inertia-  and  gravity  terms  included, 

-  No  constraints  imposed, 

-  Configuration  invariance  constraints  imposed, 

-  Gravity  balance  and  configuration  invariance  constraints  imposed. 

As  stated  earlier,  the  manipulators  were  tuned  for  zero  payload  and  the  efficiencies 
were  calculated  for  a  payload  of  1.6  kg,  in  order  to  show  the  effects  of  how  the 
manipulators’  efficiencies  change  due  to  variation  in  payload. 

The  problem  of  how  to  implement  constraints  fairly  for  both  manipulators 
had  to  be  addressed.  The  decision  was  made  to  use  the  weight  of  the  motors  to 
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balance  the  manipulators,  which  is  the  common  practice  for  robot  design,  [13]. 
This  decision  avoided  the  problem  of  having  to  add  mass  to  each  configuration  or 
changing  the  baseline  dimensions  except  those  required  to  support  the  relocated 
motors. 

3. 1  7]  Presentation  Method 

Tf  data  was  compared  by  graphical  means.  The  plots  generated  show  a  three 
dimensional  representation  of  the  efficiency  as  it  varies  over  the  x-y  workspace  (first 
quadrant).  All  efficiencies  were  calculated  for  a  smaU  change  in  the  y  direction  of 
0.1  meter  of  the  pointmass  payload  (1.6  kg)  located  at  the  end  the  manipulator 
arm.  Initial  evaluations  determined  that  quadrants  other  than  the  first  quadrant 
were  symmetrically  similiar,  therefore  it  was  unnecessary  to  compute  efficiency  for 
the  remaining  quadrants. 

The  planar  workspace  of  the  manipulators  is  an  annular  area  with  inside 
radius  equal  to  {h  —  h),  (0.3073  meters),  and  outside  radius  equal  to  (/i+/j),  (1.0337 
meters).  Because  the  base  cartesian  coordinates  were  being  used,  the  numerical 
analysis  program  calculated  rectangular  sections  of  the  workspace.  Shaded  areas 
of  the  plots  where  the  efficiency  was  not  calculated  are  set  to  be  zero.  For  example, 
in  Figure  3.1,  a  region  made  up  of  two  rectangular  areas  is  shown  to  have  a  zero 
value. 

Portions  of  this  region  are  reachable  by  the  manipulator,  but  because  a  gen¬ 
eral  trend  of  the  efficiency  was  being  sought  it  was  not  necessary  to  compute  the 
efficiency  for  these  areas.  The  dotted  line  enclosing  the  box  of  the  plot  is  included 
to  give  a  reference  point  for  the  largest  magnitude  of  that  graph. 

The  purpose  of  these  plots  is  not  to  be  able  to  interpolate  numbers  from 
them,  but  rather  to  see  the  trend  of  how  the  efficiency  varies  over  the  workspace, 
and  to  compare  the  efficiency  of  the  serial  and  parallel  configurations.  Therefore, 
the  actual  magnitude  of  any  individual  point  on  the  workspace  is  of  lessor  im- 
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Figure  3.1.  Exaunple  of  an  Efficiency  Ratio  Plot 


portantance.  The  line  of  demarcation  for  the  plots  of  x)  is  r)  =  1.0.  When  i) 
is  greater  than  one,  the  serial  arm  is  more  efficient.  When  r]  is  less  than  one, 
the  parallel  arm  is  more  efficient.  In  Figure  3.2,  for  most  of  the  workspace  the 
value  of  t;’  is  less  than  one,  indicating  a  more  efficient  parallel  manipulator.  But 
in  one  semi-circular  region,  rj  is  equal  to  one,  indicating  equal  efficiency  between 
the  two  configurations.  In  some  cases,  Figure  3.5  for  example,  the  demarcation 
line  is  difficult  to  see  because  of  large  magnitude  data  points  causing  spikes  in 
the  surface  generated.  When  this  occurs,  an  additional  plot  is  included  to  show 
how  the  efficiency  behaves  over  a  majority  of  the  workspace  showing  where  the 
value  of  one  falls.  Additional  plot  generation  is  done  by  narrowing  the  amount  of 
workspace  shown,  usually  part  of  the  y  axis,  and  eliminating  the  data  causing  the 
spiked  behavior. 
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3.2  Masslcis  Systems 

The  first  step  in  this  efficiency  analysis  was  to  reconfirm  the  work  of  Song 
and  Lee,  [14].  This  was  done  by  integrating  the  equations  of  motion  to  obtain  the 
work.  The  general  expression  for  work  is, 


(3.10) 


In  order  to  evaluate  using  the  Song  and  Lee  assumptions,  the  equations  of  motion 
had  to  specialized  for  a  massless  system.  For  the  serial  manipulator  these  equations 
are, 

Ti  =  iVoglicosOi  +  mol2gcos{0i  —  O2)  (3-11) 

'’’2  =  mogl2coa{6i  +  ^2)  (3.12) 


The  total  work  done  by  the  robot  is  the  sum  of  the  absolute  values  of  work  done  by 
each  actuator.  The  work  done  by  the  joint  one  actuator  of  the  serial  manipulator 
is, 

Wi,  =  f  ^ [trXogliCoaOi Tnogl2cos{0i  +  92)]fi^i  (3.13) 

J»U 

The  trajectory  of  the  payload  in  the  Song  and  Lee  analysis  (and  for  all  efficiencies 
calculated  in  this  analysis)  held  x  in  cartesian  coordinates  constant.  From  the 
forward  kinematic  equation: 


X  =  liCOsOi  +  l2COs{9i  -1-  02) 


(3.14) 


one  can  solve  for  cos{9i  +  ^2)  and  the  integrand  is  then  in  terms  of  the  variable  ^i. 
The  integral,  evaluated  from  initial  to  final  values,  becomes: 


Wi,  =  ^  |m„^/iCos^i -f  m<,^/2  j 

integrating  this  expression  results  in. 


d9i 


(3.15) 


Wu  =  rnogx[9if  - 


(3.16) 
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which  is  in  agreement  with  Song  and  Lee,  [14].  The  expression  work  of  the  joint 
two  actuator  of  the  serial  manipulator  is, 


W2.  =  f'"' [moghcosiOr  +  e2)]de2  (3.17) 

To  integrate  this  expression  the  constraint  equation.  Equation  3.14,  was  differen¬ 
tiated  and  solved  for  d$2- 

lisinBi 


d02 


-dOi  —  d9i 


^  ~l2ain{Bi 62)' 

and  from  the  trigonometric  identity  sin^6  4-  cos^O  =  1, 


1 


(i;  - 


Now  substituting  for  d02  and  sin{0i  -|-  ^2)1  the  integral  becomes, 


=  rriogli  f 

JtU 


(i  -jLcoser)pine, 

■)] 


(3.18) 


(3.19) 


d0i  (3.20) 


Making  use  of  a  common  substitution,  this  integral  evaluates  to, 

^2,  =  +  ^2)]  1,^  -  ^(^1/ -  ^i<)  +  (3.21) 

This  is  also  in  agreement  with  Song  and  Lee,  (14). 


The  equations  of  motion  for  the  parjiUel  manipulator  can  be  integrated  di¬ 
rectly  and  result  in  the  following  work  expressions, 


Wip  =  mogli{»in$ij  —  sinOu)  (3.22) 

W2p  =  m„gl2[3in{Bif  -I-  ^2/)  -  3in{Bu  +  ^2,)]  (3.23) 

which  again  agrees  with  Song  and  Lee,  (14). 

However,  Song  and  Lee  make  assumptions  on  signs  of  the  the  work  expres¬ 
sions  and  draw  conclusions  from  those  assumptions.  Song  and  Lee  claim  that 
the  parallel  manipulator  is  100%  efficient  based  on  evaJuation  of  one  point  in  the 
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workspace  where  the  signs  of  the  work  expressions  agree  with  their  assumptions. 
However,  from  this  analysis  it  is  seen  that  the  parallel  manipulator  is  not  100% 
efficient  for  all  the  work  space.  The  work  done  by  the  two  actuators  of  the  parallel 
manipulator  are  not  always  opposite  in  sign  which  would  lead  to  %  100  efficiency  in 
Song  and  Lee’s  analysis,  [14].  It  can  be  shown  that  in  some  parts  of  the  workspace, 
particularly  the  outer  edge,  that  the  efficiency  of  the  massless  parallel  manipulator 
falls  below  the  %100  percent  efficient  mark. 


The  first  of  the  three  dimensional  plots.  Figures  3.2,  3.3,  and  3.4  show  the 
mechancial  efficiency  of  the  massless  serial  and  parallel  configurations  for  the  first 
quadrant  of  the  robot  workspace.  Figures  3.2  and  3.3  are  plots  of  the  actual 
efficiency,  i.e. 


output  work 
input  work 
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Figure  3.2.  Efficiency  of  Massless  Serial  Manipulator 


«md  Figure  3.4  is  a  plot  of  the  comparison  of  the  efficiency  ratio, 

'  ^terial 

V  =  - 

^paralM 


(3.25) 
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Figure  3.3.  Efficiency  of  Massless  Parallel  Manipulator 


As  seen  in  Figure  3.3,  the  parallel  manipulator  is  not  100%  efficient  for  all 
area  of  the  reachable  workspace.  When  the  manipulator  is  near  the  outer  edge  of 
the  workspace,  near  point  y=1.0,  x=1.0,  the  efficiency  ratio  falls  below  the  100% 
mark.  This  less  than  perfect  efficiency  is  also  seen  when  the  manipulator  is  bent 
back  toward  its  base,  i.e.,  y=0.3,  x=0.0.  Figure  3.4  shows  the  rj'  plot  for  the 
massless  case. 

This  plot  shows  that  the  parallel  manipulator  is  more  efficient  than  the  serial 
for  the  majority  of  the  workspace.  The  level  spot  on  the  graph,  located  near 
x=0.0,  y=0.5,  has  a  magnitude  of  1.0.  Except  on  this  plane,  the  magnitude  of  rj'  is 
mostly  less  than  1.0.  Note  that  the  change  in  the  y  direction,  used  in  all  the  work 
calculations  is  0.1  meters. 

This  efficiency  analysis,  Figure  3.4,  actually  tells  us  more  than  just  confirming 
the  work  of  Song  and  Lee.  If  the  massless  manipulators  were  gravity  compensated 
for  some  nominal  payload,  the  contribution  of  the  mass  of  the  manipulator  and 
the  nominal  payload  would  be  eliminated  by  that  compensation.  Therefore,  these 
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Figure  3.4.  t]'  of  Massless  Configurations 


plots  also  show  how  the  gravity  compensated  configurations  are  effected  by  some 
variation  in  the  payload  away  from  the  nominal.  Note  that  the  efficiency  is  a 
function  of  the  location  in  the  workspace. 

In  general,  looking  at  the  rj'  in  Figure  3.4,  the  total  work  done  by  the  parallel 
manipulator,  for  a  delta  payload  away  from  some  nominal  payload  used  for  grav¬ 
ity  compensation,  is  less  than  the  serial  manipulator  for  most  of  the  workspace. 
Therefore,  Song  and  Lee’s  conclusion  that  the  parallel  configuration  is  more  effi¬ 
cient  than  the  serial  configuration  is  for  a  general  application  correct,  except  when 
the  manipulator  is  operating  in  United  parts  of  workspace. 

With  Song  and  Lee’s  work  reproduced  and  expanded  for  the  entire  workspace, 
the  next  step  was  to  add  in  the  effects  of  the  mass  of  the  manipulators.  For  this 
analysis,  the  only  performance  parameter  with  real  meaning  is  rj' . 
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3.3  GravUational  Effects 


The  next  step  was  to  add  in  the  effects  of  the  mass  of  the  configurations.  This 
addition  includes  the  link’s  mass  and  inertia.  Initially,  only  gravitational  terms  of 
the  equations  were  included.  The  work  expressions  were  still  obtainable  in  closed 
form.  The  integration  follows  identically.  The  only  changes  are  the  added  mass 
and  length  terms  of  the  arm,  which  are  a  constant  to  the  integration.  The  work 
expressions  are  as  follows: 
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Figure  3.5  plots  the  q'  efficiency  for  the  first  quadrant  of  the  workspace  when  only 
gravitational  effects  of  manipulator  mass  were  included. 


As  stated  in  section  3.1,  it  is  difficult  to  indicate  where  the  demarcation  line 
of  when  27^  =  1.0  occurs  in  Figure  3.5.  For  clarification,  Figure  3.6  is  included. 


Figure  3.6  shows  the  majority  of  quadrant  one  of  the  workspace  with  the  data 
causing  the  spiking  effect  in  Figure  3.5  eliminated.  From  examining  Figure  3.6  one 
can  conclude  that  when  arm  gravitational  dynamics  are  included  in  the  q  analysis 
the  parallel  configuration  is  more  efficient  than  the  serial  configuration  for  a  larger 
portion  of  the  workspace  than  when  massless  configurations  were  examined.  The 
serial  manipulator  is  still  more  efficient  when  the  arm  is  near  the  point  x=0.3. 
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Figure  3.5.  Efficiency  with  Arm  Mass,  (Gravity  Terms  Only) 


y=0.0.  Comparing  Figures  3.4  and  3.6,  the  conclusion  is  made  that  significant 
improvement  in  efficiency  is  obtained  by  applying  gravity  compensation  to  either 
configuration,  serial  or  parallel.  However,  even  with  this  improvement  in  efficiency, 
the  parallel  configuration  is  still  more  efficient,  except  in  a  very  limited  region  of 
the  workspace.  This  completes  the  computation  of  gravitational  terms.  Now,  the 
inertial  terms  of  the  equations  of  motion  need  to  be  included. 


S.^  Inertial  Effects 

To  determine  the  work  for  the  the  case  when  inertial  terms  are  included,  a 
closed  form  solution  was  not  possible.  Therefore,  numerical  integration  had  to  be 
used.  The  torques  could  be  calculated  from  the  equations  of  motion  at  any  given 
position  Sind  acceleration.  The  acceleration  was  assumed  to  be  an  instantaneous 
step  of  magnitude  equad  to  that  of  gravity  in  order  to  compare  the  magnitude  of 
inertia  effects  to  that  of  gravity.  The  numerical  integration  method  used  was  a 
trapezoid  rule  method,  which  assumes  that  torque  changes  linearly  between  sub- 
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Figure  3.6.  Efficiency  with  Arm  Mass,  (Gravity  Terms  Only,  Partial  Workspace) 

sequent  positions,  [5].  This  method  was  tested  by  numerically  integrating  the 
gravity  terms  and  comparing  the  result  to  the  closed  form  solution  obtained  from 
that  analysis.  This  method  was  found  to  be  acccurate  to  within  at  least  three 
significant  figures. 

The  efficiency  was  determined  including  only  inertia  terms,  without  any  “tun¬ 
ing”,  and  assuming  that  the  centers  of  gravity  for  the  links  are  located  half  way 
down  the  link  and  are  shown  in  Figure  3.7. 

For  all  of  the  workspace  in  Figure  3.7,  the  parallel  configuration  is  more  effi¬ 
cient  than  the  serial  when  mass  effects  for  only  inertia  terms  are  considered.  Figure 
3.8  represents  t]  cosidering  only  inertia  terms  when  the  configuration  invariance 
constraints  were  imposed. 

Comparing  Figure  3.8  to  Figure  3.7,  shows  that  the  overall  magnitude  of  rj' 
throughout  the  workspace  has  decreased.  This  indicates  the  efficiency  of  the  par¬ 
allel  configuration  has  increased  more  than  that  of  the  serial  when  configuration 
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Figure  3.7.  Efficiency  of  Inertia  Terms  Only,  (No  Tuning) 


invariance  is  imposed  on  the  inertial  terms  of  the  equations  of  motion.  The  next 
step  in  the  analysis  is  to  combine  the  effects  of  gravity  and  inertia,  look  for  the 
dominate  effects,  and  implement  configuration  invariance  and  gravity  compensa¬ 
tion. 


Combined  Effects 


For  the  purpose  of  this  investigation  the  combined  effects  refers  to  the  case 
when  both  the  inertial  and  gravitational  terms  of  the  equations  of  motion  are 
considered  together  in  the  computation  of  efficiency.  The  baseline  for  comparison 
was  the  nominal  case  where  no  tuning  was  implemented  and  all  centers  of  gravity 
of  the  links  were  located  at  link  midpoint.  The  efficiency  plot  for  this  baseline 
case  is  in  Figure  3.9  and  is  called  the  nominal  case  of  the  combined  efficiency. 
Agsun,  the  line  of  demarcation  between  the  efficiencies  of  the  two  configurations  is 
unclear.  Figure  3.10  showing  the  majority  of  the  workspace  and  eliminating  the 
spiked  behavior  is  included  in  order  to  better  visualize  the  demarcation  line. 
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Figure  3.8.  Efficiency  of  Inertia  Terms  Only,  (With  Tuning) 


Figure  3.10  shows  that  the  parallel  configuration  is  the  most  efficient  for  the 
majority  of  the  workspace.  The  serial  manipulator  is  more  efficient  near  the  point 
x=0.3,  y=0.6,  which  was  previously  seen  in  the  case  when  only  gravitational  terms 
were  considered. 

Using  the  weights  of  the  motors  as  counter  balances,  the  link  lengths  were 
adjusted  to  achieve  gravity  compensation.  For  the  serial  manipulator.  Figure  1.1, 
this  meant  extending  link  2  behind  the  pivotal  point  (joint  2)  by  0.1455  meters, 
satisfying  Equation  2.15.  Link  1  was  extended  by  0.4483  meters  to  satisfy  the 
condition  of  equation  2.17.  The  additional  weight  added  by  the  extension  was 
added  at  constant  mass  per  unit  length  equal  to  1.9885  kg/meter.  This  mass  per 
unit  length  constant  was  obtained  by  using  the  density  of  a  common  aluminum 
used  in  manufacturing  (  p  =  2.8  x  lO’fc^/m®)  multiphed  by  the  cross  sectional 
area  of  the  links  held  constant  throughout  this  analysis,  see  Figure  2.1.  The  motor 
mass  was  considered  constant  for  all  motors  (m„,  =  6  kg). 
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Figure  3.9.  Combined  Efficiency,  (Nomin&I  Case) 


For  the  parallel  manipulator,  Figure  1.2,  link  1  was  extended  0.0895  meters 
to  satisfy  the  gravity  balancing  constraint.  Equation  2.21.  To  satisfy  the  constraint 
for  link  2,  Equation  2.22,  link  2  did  not  have  to  be  extended,  but  rather  the  motor 
was  located  0.0466  meters  along  the  existing  link  away  from  the  base.  With  these 
constraints  enforced,  the  efficiency  is  plotted  in  Figure  3.11. 

The  manipulators  are  gravity  balanced  for  zero  payload,  and  the  efficiency 
calculated  in  Figure  3.11,  is  for  a  payload  of  1.6  kg.  Therefore,  the  plots  show 
effect  of  a  variable  payload  on  the  parameter  of  comparison  -q  .  From  Figure  3.11 
it  is  seen  that  with  the  improvement  afforded  by  gravity  compensation,  although 
the  efficiency  of  both  configurations  has  improved,  (see  section  3.2),  the  parallel 
configuration  is  the  most  efficient  over  all  the  workspace. 

In  addition  to  gravity  compensation,  the  configuration  invariance  constraint 
was  imposed  on  the  manipulators  and  the  efficiency  calculated.  This  constraint  of 
tuning  for  configuration  invariance  is  only  an  additional  constraint  to  the  parallel 
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Figure  3.10.  Combined  Efficiency,  (Nominal  Case,  Partial  Workspace) 

manipulator,  Equation  2.20,  because  satisfying  the  gravity  balancing  constraint 
for  the  serial  manipulator  also  gives  configuration  invariance.  With  this  addition, 
efficiency  was  computed  and  is  plotted  in  Figure  3.12. 

Comparing  Figures  3.11  and  3.12  it  is  seen  that  the  magnitude  of  tj’  has 
decreased  when  configuration  invariance  was  apphed  to  the  parallel  configuration. 
The  efficiency  of  the  serial  arm  was  unchanged.  Looking  again  at  the  definition  of 
efficiency  used; 

,  Input  Work  of  the  Parallel  Arm 

^  Input  Work  of  the  Parallel  Arm 

it  is  seen  the  only  the  work  done  by  the  parallel  arm  has  changed.  Therefore,  if  tj' 
decreases  in  magnitude,  the  work  done  by  the  parallel  arm  has  decreased  making 
it  more  efficient. 
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Figure  3.11.  Combined  Efficiency,  Gravity  Balanced  Case 


3.6  Summary 

This  chapter  looked  at  how  the  mechanical  efficiency  was  calculated  and  ap¬ 
plied  to  the  parallel  and  serisJ  robotic  configurations.  The  efficiency  was  detemined 
considering  the  following  cases: 


•  Only  gravity  terms  of  the  equations  of  motion. 

—  No  arm  mass  included. 

—  Arm  mass  included. 

•  Inertia  terms  only. 

-  No  configuration  invariance  tuning  applied. 

-  Configuration  invariance  tuning  applied. 

•  All  terms  of  the  equations  of  motion  included. 

-  Nominal  case  -  no  tuning. 
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Figure  3.12.  Combined  Efficiency,  Gravity  Balanced  and  Configuration  Invariant 

-  Gravity  compensated  case. 

-  Gravity  compensated  and  configuration  invariant  case. 

From  the  analysis  of  the  data  generated,  one  can  draw  three  basic  conclusions. 

1.  In  general  the  parallel  manipulator  performs  more  efficiently  than  the  serieil 
manipulator  over  a  majority  of  the  workspace. 

2.  Gravity  balancing  significantly  enhances  the  performance  of  a  manipulator. 

3.  Trying  to  achieve  minimal  control  complexity  through  configuration  invari¬ 
ance  and  dynamic  decoupling  (achievable  only  by  the  parallel  manipulator) 
results  in  improved  efficiency  of  the  manipulator.  Miinimal  control  complex¬ 
ity  is  not  achieved  due  to  the  return  of  non-linear  effects  arising  from  carrying 
a  payload  other  than  nominal. 

In  examination  of  the  plots.  Figures  3.2  through  3.12,  for  the  majority  of  the 
workspace  the  efficiency,  ,  is  less  than  one  in  value.  Looking  at  the  definition 
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of  r]\  Equation  3.1,  this  data  can  be  interpreted  to  mean  that  the  magnitude  of 
the  total  work  done  by  the  parallel  manipulator  is  less  than  that  of  the  serial 
manipulator  at  those  points  in  the  work  space.  Because  the  points  where  the 
value  of  x]  dominate  the  workspace  for  all  cases  considered,  it  can  be  stated,  by 
this  definition  of  efficiency,  that  the  parallel  manipulator  is  more  efficient.  This 
statement  includes  the  case  of  both  manipulators  “tuned”  for  a  specific  payload 
and  the  actual  load  was  not  at  that  specific  value,  Figure  3.12. 

When  comparing  gravity  compensated  data,  Figures  3.4  and  3.11,  to  the 
uncompensated  data.  Figures  3.5  and  3.9,  that  the  total  work  done  by  both  of  th® 
manipulators  decreases  significantly.  From  this  data  it  can  be  concluded  that,  if 
achievable,  gravity  compensation  is  a  worthwhile  prospect. 

Finally,  when  examining  Figures  3.12  and  3.11  one  can  see  that  the  work 
done  has  decreased  for  the  parallel  manipulator  when  making  the  change  to  con¬ 
figuration  invariance,  all  other  parameters  being  the  same.  The  serial  manipulator 
parameters  were  not  changed  because  it  achieves  configuration  invariance  when  the 
gravity  compensation  is  apphed.  Therefore,  the  lower  magnitude  of  t/'  can  be  at¬ 
tributed  to  the  improvement  in  parallel  manipulator  efficiency  when  the  conditions 
for  configuration  invariance  are  apphed.  The  conclusion  is  that  minimum  control 
complexity  can  be  achieved,  and  results  in  the  most  efficient  configuration. 
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IV.  Conclusions  and  Recommendations 

4.1  Conclusions 

A  method  was  developed  for  determining  and  comparing  the  efficiencies  of 
different  robotic  manipulator  configurations.  Two  robotic  manipulator  configura¬ 
tions  have  been  compared  to  determine  which  would  be  best  suited  for  application 
to  the  A^RM  project.  The  project  requires  a  manipulator  designed  for  general  ap¬ 
plications,  including  the  abiUty  to  handle  variable  mass  payloads.  Current  research 
in  efficiency  enhancement  has  generally  been  applied  to  manipulator  configurations 
having  constant  or  zero  mass  payloads.  The  mechanical  efficiency  of  a  open  link 
serial  kinematic  chain  manipulator  was  compared  to  the  mechanical  efficiency  of  a 
closed  hnk  parallel  kinematic  chain  manipulator  considering  the  effects  of  carrying 
a  mass  payload  other  than  that  for  which  the  manipulator  was  tuned.  The  results 
of  that  comparison  are  as  follows: 

1.  The  parallel  manipulator  performs  more  efficiently  than  the  serial  over  a 
majority  of  the  workspace. 

2.  Gravity  compensation  significantly  enhances  the  efficiency  of  each  manipu¬ 
lator. 

3.  Through  configuration  invariance  minmal  control  is  achieved  and  the  effi¬ 
ciency  of  the  parallel  configuration  manipulator  is  enhanced. 

4.2  Recommendations 

Although  thiS  research  is  significant,  this  is  only  the  first  step.  The  technique 
for  evaluation  technique  is  now  in  place,  and  further  studies  in  this  area  could  be 
fruitful. 
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•  A  form  of  the  parallel  manipulator  would  be  the  best  choice  of  the  two  con¬ 
figurations  considered,  in  terms  of  efficiency.  Consideration  of  other  configu¬ 
rations  before  making  a  final  decision  may  be  warranted. 

•  Mass  balancing  to  achieve  gravity  compensation  may  not  be  the  best  ap¬ 
proach.  Spring  counterbalancing  and  controUed-force  counterbalancing  should 
also  be  investigated. 

•  Other  trajectories  should  be  investigated,  including  short  trajectories  in  the 
x-direction,  and  long  trajectories  in  both  y  and  x-directions. 

•  An  effort  should  be  made  to  research  other  performance  measures,  such  as 
power  based  efficiency. 

•  The  next  logical  extension  would  be  to  add  in  the  coriolis  and  centrifugal 
terms  to  this  method  of  analysis,  and  then  extend  to  three  degrees  of  freedom. 

•  A  study  could  be  done  trading  off  the  size  of  motor  used  in  the  efficiency 
analysis,  (i.e.  motors  of  different  weight) 

•  Contours  of  constant  rj'  in  the  x-y  workspace  could  be  used  to  better  quantify 
the  analysis. 

•  AFIT  would  benefit  tremendously  by  having  a  working  copy  of  the  auto¬ 
matic  symbolic  equations  of  motion  generator  in  MACSYMA,  [1.5]  for  the 
tree  structure  manipulators.  This  would  benefit  instruction,  control  research, 
and  computer  dded  design  efforts  at  AFIT. 

These  are  the  recommendations  for  further  studies  in  this  area  for  AFIT.  This 
analysis  is  very  diversified  and  could  be  applied  in  many  ways. 
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Appendix  A.  Dynamics 


This  appendix  looks  expressly  at  the  dynamic  equations  of  motion  of  the 
manipulator.  The  approach  of  using  Lagrange  multipliers  [10]  is  used  to  derive 
these  equations  for  closed  Unk  kinematic  chain  manipulators.  The  equations  are 
derived  symbohcally  using  MACSYMA,  an  mathematical  symbolic  manipulation 
programming  tool,  developed  by  M.  1.  T.,[15],  to  be  able  to  look  at  the  individual 
terms  of  different  closed  link  chain  configurations.  In  examining  these  individ¬ 
ual  terms,  the  manipulator  can  be  designed  so  that  configuration  invariance  and 
dynamic  decoupling  can  be  achieved  [Ij. 

A.l  Symbolic  Equaiions  of  Motion 

Open  kinematic  chain  manipulators  have  a  joint  for  every  degree  of  freedom  in 
the  workspace.  Closed  kinematic  chains  have  at  least  one  redundant  joint  effecting 
the  motion  of  the  manipulator.  The  problem  discussed  here  is  one  where  the 
closed  form  solution  of  the  dynamic  equations  of  motion  for  the  closed  kinematic 
chain  manipulator  is  sought.  This  closed  form  solution  should  be  expressed  in  the 
least  number  of  coordinates  as  possible,  i.e.,  equal  to  the  number  of  degrees  of 
freedom.  This  enables  the  manipulator  to  be  built  without  actuation  or  sensing  on 
the  redundant  joints. 

The  method  discussed  here,  for  the  formulation  of  the  closed  form  equations, 
is  based  on  the  Lagrangian  formulation  which  has  been  developed  by  J.  Y.  S.  Luh 
and  Yuan-Fang  Zheng  in  their  paper  “Computation  of  Input  Generalized  Forces  for 
Robots  with  Closed  Kinematic  Chain  Mechanisms”,  [lOj.  Using  this  formulation 
requires  developing  the  dynamic  equations  of  motion  for  serial  Unks  which  is  done 
symbolically  using  the  MACSYMA  program  implementing  the  Lagrange- Euler  for¬ 
mulation  developed  by  M.  C.  Leu  and  N.  Hemati.  This  program  is  found  in  Leu 
and  Hemati ’s  paper  “Automated  Symbolic  Derivation  of  Dynamic  Equations  of 
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Motion  for  Robotic  Manipulators”,  [9],  and  was  implemented  at  AFIT  in  February 
1988. 


Using  these  two  papers  as  a  foundation  I  then  had  to  add  addtional  MAC- 
SYMA  programming  to  implement  the  formulation  of  Luh  and  Zheng  [10]  and 
obtain  the  symbolic  equations  of  motion  for  closed  kinematic  chain  manipulators. 

A.l.}  Lagrangian  Formulation.  The  Lagrangian  formulation  presented  by 
Luh  and  Zheng  [10]  develops  the  dynamic  equations  of  motion  for  the  closed  kine¬ 
matic  chain  manipulator  using  Lagrange  multipUers  and  assuming  that  each  joint 
senses  position,  velocity,  and  acceleration. 

Going  back  to  first  principles  [11],  the  Lagrangian  is  defined  as; 


L  ~  T  -  V  (A.l) 

where  T  is  the  total  kinetic  energy  of  the  system  and  V  is  the  potential 
energy  of  the  system.  Using  Lagrange’s  variational  calculus  techniques  to  develop 
the  equations  of  motion,  the  forces  are  placed  into  the  categories  of  conservative 
and  non-conservative  forces.  Lagrange’s  equations  can  be  written  in  the  form: 


£ 

dt 


~  Qnc  +  y)  >^mi  (i  =  1 . . .  n) 


(A.2) 


where  Qne  is  the  non-conservative  generalized  force,  A|  are  the  Lagrange  multipliers. 


and  the  an  are  the  holonomic  constraint  equations.  The  holonomic  contraints  are 


obtained  from  the  physical  constraint  equations  through  the  relationsliip: 


an  = 


dqi 


(A.3) 


where  gi  are  the  physical  contraints  determined  by  the  system  in  question.  Sub¬ 
stituting  equation  A.3  back  into  equation  A.2  and  using  vector  notation  produces 
the  equation  consistent  with  Luh  and  Zheng  [10]: 
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Dq  +  h  +  g  =  T  + 


(A.4) 


where  the  left  hand  side  of  Equation  A.4  is  from  the  conservative  force  side  of  La¬ 
grange’s  equations,  i.e.,  the  left  hand  side  of  Equation  A. 2,  f  is  the  non-conservative 
control  torque  vector,  and  g  are  the  physical  constraint  equations. 


Applying  Lagrange’s  equation.  Equation  A.4,  to  the  robotic  manipulator  with 
n  joints  (not  necessarily  independent)  results  in  n  equations.  But  the  joint  coor¬ 
dinates  are  related  through  the  m  physical  constraint  equations,  i.e.  g.  Therefore, 
the  system  of  equations  we  have  developed  is  the  n  Lagrange’s  equations  with  n 
-f  m  unknowns,  n  unknowns  are  from  the  coordinates  and  m  unknowns  are  from 
the  Lagrange  multiphers,  and  the  m  physical  constraint  equations  which  have  the 
n  unknowns  of  the  joint  coordinates. 


Now  the  system  of  equations  can  be  solved. 


But  these  equations  apply  only  to  open  kinematic  chain  chain  manipulators 
that  are  somehow  physically  constrained.  To  apply  this  development  to  a  closed 
kinematic  chain  manipulator  Luh  and  Zheng  {10]make  a  virtual  cut  at  a  joint 
in  the  closed  chain  that  is  not  actuated.  This  results  in  two  open  kinematicc 
chains  that  have  a  common  point  at  the  cut.  A  position  vector  from  the  base  to 
the  cut  defines  that  point.  Tliis  vector  is  defined  by  both  open  chains  through  a 
series  of  homogeneous  transformations  from  the  base  to  the  cut.  The  homogeneous 
transformation  matrix  are  contructed  using  the  Denavit-Hartenburg  representation 
[3,  page  36]  and  will  be  of  the  form: 


T  =  {n,5,a,^  (A.5) 

where  n,  s,  and  a,  represent  the  rotation  of  the  coordinate  frame  of  the  cut  joint 
from  the  base  coordinate  frame,  and  p  is  the  position  vector  vector  from  the  origin 
of  the  base  frame  to  the  cut  expressed  in  the  base  frame  coordinate  system. 
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Wlieii  file  virtual  cut  is  done  at  a  revolute  joint,  the  p  vector  and  the  a  vector 
is  equal  for  both  open  chsun  transformations.  The  only  difference  would  be  some 
constant  physical  offset  length  between  the  origins  of  the  coordinate  frames  at  the 
cut  depending  on  how  those  coordinate  frames  are  defined. 

These  two  position  vectors,  p,  give  us  the  physical  contrwnt  Equations,  g, 
required  for  Lagrange’s  equations.  Equation  A.4.  The  components  of  the  p  vectors 
are  set  equal  to  each  other  yielding  the  m  constraint  equations. 

The  next  step  in  the  Lull  and  Zheng  application  [10]  is  set  the  non-conserative 
torques  for'unactuated  joints  equal  to  zero.  The  order  of  the  n  equations,  Equation 
A. 4,  is  then  changed  to  reflect  that  the  equations  with  actuated  joints  appear  in 
the  1  through  n-m  section  of  the  array,  and  the  equations  derived  with  respect  to 
joints  without  actuators  appear  in  the  n-m+l  through  n  section  of  the  array.  Now 
the  equation  array  in  the  n-m+l  through  n  section  can  be  solved  for  the  Lagrange 
multipliers: 


(A.6) 


Having  solved  for  the  Lagrange  multipliers,  these  expression  are  then  substi¬ 
tuted  into  the  1  through  n  equations  of  motion  resulting  in  an  expression  for  the 
control  torques  of  the  actuated  joints,  t.  This  expression  can  be  written  as: 


T  = 


/  \l  (^\  _  ^1  _  1 

ikvw  1^9/  /i, 


(A.7) 

thru  (n-m) 

where  t  are  the  generalized  or  “constrained”  equations  of  motion  found  in  Luh  and 
Zheng  [10].  This  is  a  system  of  n-m  equations  of  motion  with  n  joint  coordinate 
unknowns.  The  dependent  coordinates,  n-m  of  them,  can  be  solved  for  from  the 
constraint  equations,  g.  Another  method  for  solving  equation  A.7  would  be  to 
sense  position,  velocity,  and  acceleration  on  all  joints. 
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The  ultimate  closed  form  solution  would  be  obtained  by  eliminating  the  de¬ 
pendent  coordinates.  This  is  done  by  solving  the  constraint  equations  for  the 
dependent  coordinates,  their  rates,  and  their  accelerations  and  substituted  into 
Equation  A. 7. 

A.  1.2  Implementation.  To  apply  the  Lagrangian  formulation  presented  by 
Lull  and  Zheng  [10],  the  Leu  and  Hemati  [9]  MACSYMA  program  was  used  to 
symbolicaJly  derive  the  equations  of  motion  for  the  open  kinematic  chains.  The 
program  is  included  as  follows: 

/*  This  program  darivas  tha  aquation  of  notion  of  a  manipulator 
link  using  tha  Lagrangian  formulation. 

INPUT - > 

DOF  ;  MO.  OF  DEGREES  OF  FREEDOM. 

JOINT  :  THE  TYPE  OF  JOINT  (0  FOR  RETOLUTE, 

I  FOR  PRISMATIC). 

d. a. alpha  :  LINK  GEOMETRIC  PARAMETERS. 

R  :  LINK  MASS  CENTER  POSITION  VECTOR. 

M  :  LINK  NASS. 

MOM  :  LINK  PSEUDO  INERTIA  MATRIX. 

LNK  :  LINK  NUMBER. 

GF  :  GRAVITATIONAL  FIELD  VECTOR. 

OUTPUT  - > 

F[I]  ;  GENERALIZED  FORCE  AT  JO’^NT  !•/ 

TMATRIXO  :=  ( 

PRINT ("  ENTER  THE  NUMBER  OF  DEGREES  OF  FREEDOM") , DOF :READ() , 

FOR  I  THRU  DOF  DO  ( 

PRINTC’TTPE  0  IF  JOINT  IS  REVOLUTE  AND  1  IF  JOINT  IS  PRISMATIC"), 

PRINTC  ").J0INT:READ(), 

IF  J0INT=0  THEN  ( 

PRINTC'INPUT  THE  PARAMETERS  OF  THE  REVOLUTE  JOINT :d, a, alpha" ) , 

PRINTC  "),D[I]  :READ(),AD[I]:READ{),ALF[I]:READ(), PRINTC  "), 

A  [I]  :  MATRIX  (  [COS  ( Q  [I]  )  ,  -SIN  (Q  [I]  )  aCOS (  ALF  [I]  )  , 


A-5 


f 


SI«(Q[I])*SIM(ALF[I]),AD[I]*COS(Q[I])] , 
[SIll(Q[I]).COS(QtI])*COS(ALF[I]), 

-COS(q[I] )*SIH(ALF[I]) ,AD[I]*SIH(Q[I] )] , 

[0 . SIM ( ALF [I] ) , COS ( ALF [I] ) ,D  tl] ] , 

[0,0, 0,1])) 

ELSE(PRIHT("INPUT  THE  PARAMETERS  OF  THE  PRISMATIC  JOINT :th«ta, a, alpha" ) , 
PRINT ( "  " ) , TH [I] ; READ () , AD [I] : READ ( ) , ALF [I] : READ ( ) , PRINT( "  " ) , 

A[I] : MATRIX ( [COS (TH [I] ) ,-SIN{TH[I] )*COS(ALF[I] ) , 

SIN(TH[I) )*SIN(ALF[I3 ) ,AD[I]*COS(TH[I] )] , 

[SIN(TH[I] ) ,COS(TH[I] )*C0S(ALF[I3 ) , 
-C0S(TH[I])*SIN(ALF[I]),AD[I3*SIN(TH[I3)] , 
[0,SIN(ALF[I] ) ,C0S(ALF[I3) ,Q[I33 , 

[0,0,0,13))), 

/*  GENERATE  THE  T  MATRICES  */ 

FOR  I  THRU  DOF  D0( 

IF  1=1  THEN  T[I3:A[I3  ELSE  T[l3 :T[I-1] . A[I] ) ) ; 

/*  TAKE  THE  FIRST  DERIVATIVE  OF  THE 

T  MATRICES  W.R.T.  THE  JOINT  VARIABLES.*/ 

DIFFTlO  :=  (FOR  I  THRO  DOF  D0( 

FOR  J  THRU  DOF  D0( 

IF  I>=J  THEN(U[I,J]  :  DIFF(T[I] ,Q[J3 ) ) )) ) ; 

/*  TAKE  THE  SECOND  DERIVATIVE  OF  THE  T  MATRICES  •/ 

DIFFT2()  ;=  (FOR  I  THRU  DOF  D0( 

FOR  J  THRU  DOF  D0( 

IF  I>=J  THEN( 

FOR  K:J  THRU  DOF  D0( 

IF  I>=K  THEN(W[I,J,K3  :  DIFF(U[I, J] ,Q[K3 ))))))) 5 

/*  INPUT  THE  MASS  PROPERTIES  •/ 

IIBRTIAO  :=  (FOR  I  THRO  DOF  D0( 
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PRINTC'ENTER  THE  INERTIA  MATRIX  FOR  LINK  NO.  ",I), 

MOM[I]  :ENTERMATRIX(4,4),PRINT("  *’), 

PRINT ("ENTER  THE  CENTER  OF  NASS  VECTOR  FOR  LINK  NO.  ",I), 
R[I] :EMTERMATRIX(4,1) , PRINT ("  ")) , 

PRINTC’ENTER  THE  GRAVITY  FIELD  VECTOR"), 
GF:ENTERMATRIX(4.1)); 

/*  DERIVE  THE  DI  TERMS  •/ 

TERMDIO  :=  (PRINT("  ") .PRINTC’ENTER  THE  LINK  NO.  ") ,LNK:READ() , 

DI  :  0,I:LNK, 

FOR  PPI:I  THRU  DOF  DO  ( 

DI  :  DI  +  {((-MtPPI]*TRANSPOSE{GF)).0[PPI,I]).R[PPI])), 

DD[I]  :  DI): 

/*  DERIVE  THE  DIJ  TERMS  */ 

TERMDUO  :=  (I:LNK,  L:l, 

FOR  J:I  THRO  DOF  DO  ( 

TRAC  :  0,MAXIJ;J, 

FOR  P;MAXIJ  THRU  DOF  DO  ( 

JTQICP]  :  MOMtP] .TRANSPOSE (OtP, I]), 

FOR  L  THRU  4  DO  ( 

TRACI  :  TRAC  +  ROW(U[P, J] ,L) .COL( JTQI [P] ,L) , 

TRAC  :  TRACI)), 

DIJ[I,J]  :  TRAC)): 

/*  DERIVE  THE  DIJK  TERMS  •/ 

TERNDIJKO  :=  (I:LNK,  LL:1, 

FOR  J  THRU  DOF  DO  ( 

FOR  K:J  THRU  DOF  DO  ( 

IF  J=I  AND  I  >=  K  THEN 
DK[I.J,K]  :  0 

ELSEdF  (J<I  AND  J<K  AND  K>=I)  OR  (J>=I  OR  J>=K)  THEN( 

IF  I>K  THEN 

(IF  I>J  THEN  NAXIJK;!  ELSE  NAXIJK:J) 
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ELSE  MAXIJK:K. 

TRACEP  :  0, 

FOR  PP:NAXIJK  THRU  DOF  DO  ( 

JTPICPP]  :  HOM[PP]  .TRAIISPOSE(U[PP,I]). 

FOR  LL  THRU  4  DO  ( 

TRACEP  :  TRACEP  + 

ROW ( W [PP , J , K] , LL) . COL ( JTPI [PP] , LL) ) ) , 

DK[I.J,K]  :  TRACEP))))): 

/*  COLLECT  THE  DI.  DIJ,  DIJK  TERMS  TO  OBTAIN 

THE  EQUATION  OF  MOTION  OF  LINK  I.  •/ 

FIO  :=  (TRMDIJ  :  0.,TRMDIJK  :  0., 

FOR  J  THRU  DOF  D0( 

(IF  KLNK  THEN 

TRMDIJ  :  TRMDIJ  +  DIJ[J,LNK]*DDQ[J] 

ELSE 

TRMDIJ  :  TRMDIJ  +  DIJ[LNK,J]*DDQ[J]) , 

FOR  K;J  THRU  DOF  D0( 

IF  K=J  THEN 

TRMDIJK  :  TRMDIJK  *  DK[LNK,J,K]*Dq[J]*Dq[K] 

ELSE  IF  KLNK  AND  J<K  THEN 
(IF  K>=LNK  THEN 

TRMDIJK  :  TRMDIJK  +  2*DK[LNK,J,K]*Dq[J]*Dq[K] 

ELSE 

TRMDIJK  :  TRMDIJK  -  2*DK[K,J,LNK]*Dq[J]*DQCK]) 

ELSE 

TRMDIJK  :  TRMDIJK  *  2*DK[LNK, J ,K] *DQ[J] *Dq[K] )) , 

FCLNK]  :  TRMDIJ  *  lA CLNK] *DDq [LNK]  *  TRMDIJK  *  DD[LNK]); 

Having  the  open  kinematic  chain  dynamic  equations  of  motion,  additional  MAC- 
SYMA  programming  was  necessary  to: 

1.  Isolate  the  physical  constraint  equations 
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2.  Find  the  partial  derivatives  of  the  constraint  equations,  i.e.,  find  the  holo- 
nomic  constraints 

3.  Assign  the  unactuated  forces  to  the  proper  element  position  in  the  force 
vector  according  to  the  Lull  and  Zheng  paper  [10] 

4.  Solve  for  the  constrained  equations  of  motion  (eliminating  the  Lagrange  mul¬ 
tipliers,  but  still  working  with  an  excessive  number  of  coordinates). 

Finding  the  constrained  forces  can  be  done  in  a  general  sense  as  demonstrated  in 
Lull  and  Zheng  [10].  I  have  modified  the  Leu  and  Hemati  program  [9]  to  solve  for 
the  symbolic  equations  of  motion  of  the  closed  link  kinematic  chain  manipulator 
with  the  stipulation  that  the  virtual  cut  in  the  formulation  be  made  at  a  revolute 
unactuated  joint.  This  program  is  as  foUows: 

/*  Modifiad  varsion  of  program  davalopad  by  Lau  and  Hamati  for 

darivaing  aquations  of  motion  of  a  manipulator.  Tha  modification 
implamants  tha  mathod  davalopad  by  Luh  and  Zhang  for  dariving 
aquations  of  motion  for  closad  kinamatic  chains  using  tha  mathod 
of  Lagranga  multipliars.  Tha  tools  davalopad  for  finding  tha  constraint 
aquations  assuma  that  tha  virtual  cut  takas  placa  at  a  ravoluta  joint.*/ 


/*  This  program  darivas  tha  aquation  of  motion  of  a  manipulator 
link  using  tha  Lagrangian  formulation. 

IIPUT - > 

DOF  :  10.  OF  DEGREES  OF  FREEDOM. 

JOINT  :  THE  TTPE  OF  JOINT  (0  FOR  REFOLUTE, 

1  FOR  PRISMATIC). 

d. a. alpha  :  LINK  GEOMETRIC  PARAMETERS. 

R  ;  LINK  MASS  CENTER  POSITION  VECTOR. 

M  ;  LINK  NASS. 

NON  :  LINK  PSEUDO  INERTIA  MATRIX. 

LNK  :  LINK  NUMBER. 

GF  :  GRAVITATIONAL  FIELD  VECTOR. 


OUTPUT 


> 


F[I]  :  GENERALIZED  FORCE  AT  JOINT  I*/ 

TNATRIXO  :=  ( 

PRINT ("  ENTER  THE  NUMBER  OF  DEGREES  OF  FREEDOM") ,DOF:READ() , 

FOR  I  THRU  DOF  DO  ( 

PRINTC'TTPE  0  IF  JOINT  IS  REVOLUTE  AND  1  IF  JOINT  IS  PRISMATIC"), 

PRINTC  "),JOINT:READ(), 

PRINTC'INPUT  THE  DESIRED  JOINT  NUMBER"),  /•modification*/ 

PRINTC  "),  DESIREDCI];  READO,  /•mod*/ 

Q[I] :Q [DESIRED [I]] ,  /•mod*/ 

IF  J0INT=0  THEN  ( 

PRINTC'INPUT  THE  PARAMETERS  OF  THE  REVOLUTE  JOINT :d, a, alpha" ) , 

PRINTC  ")  ,D[I]  :READ()  ,ADtI]  :READ()  , ALF[I]  : READO  ,PRINTC  ") , 

A[I] : MATRIX ( [COS(q [I] ) ,-SIN(Q[I] )*COS(ALF[I] ) , 
SIN(Q[I])*SIN(ALF[I]),ADCl]*COS(Q[I])], 

[SIN(Q[I] ) ,COS(Q[I] )*COS(ALF[I] ) , 
-COS(Q[I])*SIN(ALF[I]),AD[I]*SIN(Q[I])]  , 
[O,SIN(ALF[I]),C0S(ALF[I]),D[I]], 

[0,0,0,!])) 

ELSE (PRINT ("INPUT  THE  PARAMETERS  OF  THE  PRISMATIC  JOINT :thata, a, alpha" ) . 
PRINTC  ")  ,TH[I]  :READ()  ,AD[I]  :READ()  ,ALF[I]  :READ() , PRINTC  ") . 

A [I] : MATRIX ( [COSCTH [I] ) ,-SIN(TH[I] )*C0S(ALF[I] ) , 

SIN(TH[I] )*SIN(ALF[I] ) ,1D[I]*C0S(TH[I] )] , 
[SIN(TH[I]),C0S(TH[I])*C0S(ALF[I]), 

-COS (TH [I] ) *811 ( ALF [I] ) , AD [I] •SIN (TH [I] )] , 

[0 , SIN  ( ALF  [I]  )  ,  COS (  ALF  [I]  ) . Q [I] ]  , 

[0,0,0,!]))), 

/•  GENERATE  THE  T  MATRICES  •/ 

FOR  I  THRU  DOF  D0( 

IF  1=1  THEN  T[I]:A[I]  ELSE  T[I] :T[I-1] . A[I] ) ) ; 

/•  TAKE  THE  FIRST  DERIVATIVE  OF  THE 
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T  MATRICES  W.R.T.  THE  JOINT  VARIABLES.*' 

DIFFTlO  :=  (FOR  I  THRU  DOF  D0( 

FOR  J  THRU  DOF  DO( 

IF  I>=J  THEII(U[I,J]  :  DIFF(T[I]  ,Q[J]  )))))  ; 

/•  TAKE  THE  SECOND  DERIVATIVE  OF  THE  T  MATRICES  */ 

DIFFT2()  :=  (FOR  I  THRU  DOF  DO( 

FOR  J  THRU  DOF  D0( 

IF  I>=J  THEN( 

FOR  K:J  THRU  DOF  DO( 

IF  I>=K  THEH(W[I,J,K]  :  DIFF(0[I, J]  ,Q[K] ))))))) ; 

/•  INPUT  THE  MASS  PROPERTIES  */ 

INERTIAO  :=  (FOR  I  THRU  DOF  DO( 

PRINTC'ENTER  THE  INERTIA  MATRIX  FOR  LINK  NO.  , 

MOM [I] :ENTERMATRIX(4,4),PRINT("  "), 

PRINTC'ENTER  THE  CENTER  OF  MASS  VECTOR  FOR  LINK  NO.  ",I), 

R[I] ;ENTERMATRIX(4,1),PRINT("  ")), 

PRINTC'ENTER  THE  GRAVITT  FIELD  VECTOR"), 

GF:ENTERMATRIX(4,1)); 

/•  DERIVE  THE  DI  TERMS  •/ 

TERMDIO  :=  (PRINTC  ")  .PRINTC'ENTER  THE  LINK  NO.  ")  ,LNK:READ() , 

01  :  0,I:LNK, 

FOR  PPI:I  THRU  DOF  DO  ( 

DI  :  DI  +  (((-M[DESIRED[PPI]]*TRANSPOSE(GF)).D[PPI,I]).R[PPI])), 
DD[I]  :  DI); 

/•  DERIVE  THE  DIJ  TERMS  */ 

TERMDIJO  :=  (I:LNK,  L:l, 

FOR  J:I  THRU  DOF  DO  ( 

TRAC  :  0,HAXIJ:J, 

FOR  P:MAXIJ  THRU  DOF  DO  ( 

JTQICP]  ;  MOH[P] .TRANSPOSE (U CP, I]), 
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FOR  L  THRU  4  DO  ( 

TRACI  :  TRAC  +  ROU(V[P,J],L).COL(JTQI[P],L). 

TRAC  :  TRACI)), 

DIJ[I,J]  :  TRAC)): 

/•  DERIVE  THE  DIJK  TERMS  */ 

TERMDIJKO  ;=  (I:LHK,  LL:1. 

FOR  J  THRU  DOF  DO  ( 

FOR  K:J  THRU  DOF  DO  ( 

IF  J=I  AND  I  >=  K  THEN 
DK[I,J,K]  :  0 

ELSEdF  (J<I  AND  J<K  AND  K>=I)  OR  (J>=I  OR  J>=K)  THEN( 
IF  I>K  THEN 

(IF  I>J  THEN  HAXIJK:!  ELSE  MAXIJKtJ) 

ELSE  MAXIJK:K, 

TRACE?  :  0, 

FOR  PP:MAXIJK  THRO  DOF  DO  ( 

JTPI[PP]  :  MOM [PP] .TRANSPOSE (U[PP, I]) , 

FOR  LL  THRU  4  DO  ( 

TRACEP  :  TRACE?  + 

ROW{H[PP,J,K],LL) .COL(JTPI[PP] ,LL))) , 
DK[I,J,K]  :  TRACE?))))); 

/*  COLLECT  THE  DI,  DU,  DUK  TERMS  TO  OBTAIN 

THE  EQUATION  OF  MOTION  OF  LINK  I.  */ 

FI()  ;=  (TRMDU  :  0.,TRMDUK  ;  0., 

FOR  J  THRU  DOF  D0( 

(IF  J<LNK  THEN 

TRMDU  :  TRMDU  +  DU[J,LNK]*DDQtDESIREDCJ]] 

ELSE 

TRMDU  ;  TRMDU  -f  DU [LNK,J]*DDQ [DESIRED [J]]  ) , 

FOR  K:J  THRU  DOF  00( 

IF  K=J  THEN 

TRMDUK  :  TRMDUK  *  DK [LNK  ,J  ,K] *Dq [DESIRED [J]] • 
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DQ [DESIRED [K]] 

ELSE  IF  KLNK  AND  J<K  THEN 
(IF  K>=LNK  THEN 

TRMDIJK  :  TRMDIJK  +  2*DK[LNK,J,K]* 

DQ [DESIRED [I] ] *Dq [DESIRED [K] ] 

ELSE 

TRMDIJK  :  TRMDIJK  -  2*DK[K,J,LNK]* 

DQ [DESIRED [ J] ] *Dq [DESIRED [K] ] ) 

ELSE 

TRMDIJK  :  TRMDIJK  +  2*DK[LNK,J,K]* 

Dq [DESIRED [ J] ] *Dq [DESIRED [K] ] ) ) . 

F [DESIRED [LNK]]  :  *"  JIJ  +  lA [DESIRED [LNK]]* 

DDCOE.aREDaNK]]  +  TRMDIJK  +  DD[LNK]); 

/*  END  OF  THE  LEU  AND  HEMATI  MODIFIED  CODE,  BEGINNING  OF  CODE  DEVELOPED 
SPECIFICALLT  FOR  THE  FORMULATION  OF  THE  CLOSED  CHAIN  EOMS  */ 

/•  TOOL  TO  SAVE  THE  TMATRIX  FROM  THE  BASE  TO  THE  VIRTUAL  CUT 
FOR  EACH  SERIAL  CHAIN  */ 

TCUT();=  (PRINTCINPUT  JOINT  NUMBER  AT  CUT  JOINT"), 

PRINTC  "),  I:READ(), 

TCUT[I]:  T[DOF]): 

/*  TOOL  FOR  CALCULATION  OF  ALL  POSSIBLE  CONSTRAINT  EQUATIONS. 

USER  IS  RESPONSIBLE  FOR  FORMING  CONSTRAINT  MATRIX  WITH  THE 
INDEPENDENT  CONSTRAINTS  */ 

CONSTRAINTSO  :=  (PRINTCINPUT  JOINT  NUMBERS  OF  CONNECTING  JOINTS:  i,  j"), 
PRINTC  "),  i:  READO,  J;  READO, 

PRINTCTTPE  0  IF  THE  JOINT  IS  REVOLUTE  AND  1  IF  PRISMATIC"), 
PRINTv"  "),  JOINT:  READO, 

IF  JOIHT=  0  THEN  ( 

PRINTCINPUT  THE  JOINT  OFFSET:  d"). 


PRIHTC  "),  d:  READO, 

C[l]:  TCUT[i][l,4]-TCUT[j][l,4], 

C[2]:  TCUT[i][2.4]-TCUT[j][2,4]. 

C[3]:  TCUT[i][3,4]-TCUT[j]t3,4]  -  d. 

C[4]:  TCUTti][l,3]-TCUT[j][l,3], 

C[5]:  TCUT[i][2,3]-TCUT[j][2,3], 

C[6]:  TCaT[i]C3,3]-TCUTCj]C3,33) 

ELSE  (PRIRTCIMPUT  THE  OFFSET  ARCLE:  THETA"), 
PRIHTC  "),  THETA;  READO, 

C[l]:  TCUT[i][4,l]-TCUT[j][4,l3, 

C[23:  TCUT[i3  [4,23-TCUT[j3[4,23, 

C[33:  TCUT[i3 [3,l3-TCOTtj3 [3,13 , 

C[43:  TCUT[i3[3,23-TCUT[j3[3,23, 

C[S3:  TCUT[i3  [3,33-TCUT[j3[3,33, 

C[63 :  TRAHSP0SE(C0LUMH(TCBT[i3 ,1)) . 
C0L0MH(TCUT[j3 ,1)  -  THETA)); 


/*  TOJL  TO  FIND  THE  HOLOHOMIC  COMSTRAIMT  EQUATIOHS,  THIS  IS  OHLT  FOR 
THE  CASE  WHERE  THE  VIRTUAL  COT  IS  A  REVOLOTE  JOINT*/ 

HOLCONSTRAINTSO  :=  ( 
dc[l  ,13  :diff  (c[l3  ,q[l3) , 
dc[l,23:diff(c[23,q[l3), 
dc[2,l3:diff(c[l3,q[23), 
dc[2,23:dilf(c[23,q[23), 
dc[3,l3:diTf(c[l3,q[33), 
dc[3,23:diff(c[23,q[33), 
dcl4.l3 :difl(c[l3 ,q[43) , 
dc[4,23 :dilf(c[23 ,q[43))l 

/*  TOOL  TO  REORDER  THE  EQUATIONS  OF  NOTION  IN  LUH  AND  ZHENG  FASHION  */ 

REORDEREON();=  ( 
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noforc«:matTix( [f [3]] , [1 [4]] ) , 
actf ore* : matrix ( [f [l]] , [1 [2]] ) )t 


/*  TOOL  TO  BREAK  HOLONOMIC  CONSTRAINTS  INTO  ACTUATED  AND  UNACTUATED 
SECTIONS  •/ 

DIVIDEHOLONOMICSO  :=  ( 
cn:matrix([dc[l,l] ,dc[l,2]] , 

[dc[2,l],dc[2.2]]), 
cm : matrix ([dc [3,1] ,dc[3,2]l , 

[dc[4,l],dc[4,2]]))$ 

/*  FUNCTION  TO  SOLVE  FOR  THE  LAGRANGE  MULTIPLIERS  */ 

LANBDA();=  ( 

lambda : invert ( cm ) . noforce ) I 


/*  FUNCTION  TO  SOLVE  FOR  THE  GENERALIZED  FORCES  */ 

FORCES():=  ( 
fc:actforce-cn. lambda)! 

/*  END  OF  PROGRAM  */ 

The  leist  step  of  reducing  the  equations  of  motion  to  be  functions  of  only  the  inde¬ 
pendent  coordinates  is  not  readily  generalized.  The  constraint  equations  include 
triginometric  operations  on  the  coordinates.  Therefore,  solving  for  those  coordi¬ 
nates  results  in  non-unique  solutions,  i.e.,  angles  multiplied  by  factors  of  27r. 

However,  if  all  the  joints  are  revolute  the  sines  and  cosines  of  the  coordinates 
can  be  solved  for.  From  these  expressions  the  joint  rates  and  accelrations  can 
be  found.  Then  the  equations  of  motion  can  be  reduced  to  functions  of  only  their 
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indeppiident  coordinates.  This  development  is  best  shown  by  means  of  an  example. 
Using  the  manipulator  shown  in  Figure  1.2,  the  following  constraint  equations  are 
derived; 

The  MACSYMA  generated  equations  of  motion  are  shown  in  the  foUowing 
script.  This  script  is  the  actual  MACSYMA  transaction  of  the  derivation  of  the 
equations  of  motion  for  the  parallel  bar  closed  link  configuration. 

(d2)  pbarcoeff script 

(c3)  fc[l]  .-fullratsubstCl  ,co8(q[l]  )**2+8iii(q[l]  c[l]  )$ 

Batching  the  file  /nsr/local/lib/macsyma/share/lrats .mac 

(c3)  /*  MACSYMA  */ 

EVAL_ WHEN (BATCH , TTYOFF ; TRUE) $ 

(c3)  /*ASB;LRATS  3 

5:05pm  Tuesday,  14  July  1981 

7:53pm  Saturday,  29  May  1982 

Added  a  DIAGEVAL.VERSION  for  this  file. 

1:43pm  Saturday,  12  June  1982 
Changed  loadflags  to  getversions,  DEFINE.VARIABLE: 'MODE. 


EVAL_ WHEN (TRANSLATE , 

DEFINE.VARIABLE: ’MODE, 

TRANSCOMPILE: TRUE) $ 

(c3)  PUT(’LRATS,3, ’DIAGEVAL.VERSION)^ 

(c3)  DEFINE_VARIABLE(MESSLRATS2, "Invalid  argument  to  FULLRATSUBST: " ,ANY)$ 

/usr/local/lib/macsyma/trcoisl/trmode.o  being  loaded. 

(c3)  DEFINE_VARIABLE(FULLRATSUBSTFLAG, FALSE, BOOLEAN)! 

(c3)  LRATSUBST(LISTOFEqNS ,EXP) : -BLOCK ( 

[PARTSWITCH : TRUE , INFLAG : TRUE , PIECE] , 

IF  HOT  LISTP(LISTOFEQNS) 

THEN  IF  INPART(LIST0FEQNS,0)-"-" 

THEN  LISTOFEQNS : [LISTOFEQNS] 
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ELSE  IF  FULLRATSTJBSTFLAG*TRUE 

THEN  ERR0R(MESSLRATS2 , [LISTOFEQNS ,EXP] ) 

ELSE  ERROR ("Invalid  argumant  to  LRATSUBST: " , [LISTOFEQNS.EXP] ) , 

FOR  IDUM  IN  LISTOFEQNS  DO 
IF  INPART ( IDUM, 0)#"=" 

THEN  IF  FULLRATSUBSTFLAG*TRUE 

THEN  ERR0R(MESSLRATS2, [LISTOFEQNS.EXP] ) 

ELSE  ERROR ("Invalid  argument  to  LRATSUBST: ", [LISTOFEQNS.EXP] ) , 

LRATSUBSTl (LISTOFEQNS , EXP) ) I 

(c3)  LRATSUBSTl (LISTOFEQNS, EXP) :=BLOCK( 

[DUM:IF  LISTOFEQNS* [] 

THEN  EXP 

ELSE  IF  REST(LISTOFEQNS)=[] 

THEN  RATSUBST(INPART(LISTOFEQNS ,1,2), INPART(LISTOFEQNS ,1,1) ,EXP) 
ELSE  LRATSUBSTl (REST (LISTOFEQNS ) , 

IF  FULLRATSUBSTFLAG»TRUE 

THEN  FULLRATSUBST1(INPART(LIST0FEQNS,1,2) , 

INPART (LISTOFEQNS ,1,1), 

EXP) 

ELSE  RATSUBSTdNPART (LISTOFEQNS,  1 ,2)  , 

INPART(LIST0FEQNS,1,1)  , 

EXP))], 

DECLARE(DUM, SPECIAL) , 

IF  FULLRATSUBSTFLAG=TRUE  AND  DDM#EXP 
THEN  LRATSUBSTl (LISTOFEQNS, DUM) 

ELSE  IF  DUM«EXP 
THEN  DUM 
ELSE  EXP)$ 

(c3)  FULLRATSUBSTl (SUBSTEXP.FOREXP, EXP) : -BLOCK ( 

[DUM : RATSUBST(SUBSTEXP , FOREXP , EXP)] , 

IF  DUM-EXP 
THEN  EXP 

ELSE  FULLRATSUBSTl (SUBSTEXP , FOREXP , DUM) ) $ 

(c3)  FULLRATSUBST( [ARGLIST] ) : -BLOCK( 

[FULLRATSUBSTFLAG : TRUE , LARGLISTDUM : LENGTH (ARGLIST) , FARGLIST , 

PARTSHITCH : TRUE , INFLAG : TRUE , PIECE] , 

IF  LARGLISTDUM-2 

THEN  IF  LISTP(FARGLIST:FIRST(ARGLIST))  OR  INPART(FARGLIST,0)-"-" 

THEN  LRATSUBST(FARGLIST,LAST(ARGLIST) ) 
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ELSE  ERROR (MESSLRATS2 , ARGLIST) 

ELSE  IF  LARGLISTDUM-3 

THEN  APPLY(»FULLRATSUBST1, ARGLIST) 

ELSE  ERR0R(MESSLRATS2, ARGLIST))! 

(c3)  EVAL_WHEN(BATCH.TTYOFF: FALSE)! 

Batching  done. 

(c4)  fc[2] :fullrat8nb8t(l,co8(q[l] )**2+8in(q[l] )**2,fc[2] )! 
(c5)  fc[l]  :fullrat8nb8t(l ,co8(q[2] )**2+8in(q[2]  )’**2,fc[l]  )! 
(c6)  f c [2] :fullrat8ab8t (1 ,cos(q[2] )**2+8in(q[2] )**2 ,f c [2] )! 
(c7)  fc [1] :fullrat8ubst(dqCl] -dq[2] ,dq[3] ,fc [l] )! 

(c8)  fc[2] :fullrat8ub8t(dq[l]-dq[23 ,dq[3] ,fc[2])! 

(c9)  fc[l] :fullrat8ub8t(ddq[l] -ddq[2] ,ddq[3] ,lc[l] )! 

(clO)  fc[2] :fallrat8ub8t(ddq[l] -ddq[2] ,ddq[3] ,fc[2] )! 

(cll)  fcCl]  :fiillrat8nb8t(-dqCl3+dq[2]  ,dq[4]  ,fc[l)  )! 

(cl2)  fc[2] :fullrat8nb8t(-dq[l]+dq[23 ,dq[43 ,fc[23 )! 

(cl3)  fc[l3  :fTillrat8ub8t(-ddq[l3'''ddq[23  ,ddq[43  ,lc[l3  )  ; 

(dl3)  [(1  m+gm+gm)  co8(q  )  v 

1  4  3  3  1  1  1 

+  ((ddq  1  g  III  -  1  ddq  go)  sin(q  ) 

2233  1  244  1 

2  2 

(1  dq  g  m  -  dq  1  g  m  )  coeCq  ))  8in(q  ) 
1244  2233  1  2 

2  2 

+  ((dq  Igm-ldqgm)  8in(q  ) 

2233  1244  1 
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2 

+  (ddq  1  g  m  -  1  ddq  g  m  )  cosCq  ))  cosCq  )  +  ddq  1  m 
2233  1  244  1  2  114 

2  2 
+(ddq  -  ddq  )ia  +ddq  g  m  +ddq  izz  +(ddq  -ddq  )ia  +  ddq  g  m 

1  24  133  13  1  23  111 

ddq  izz  +  ddq  ia  ] 

11  11 

(cl4)  fc[2] :fullrat8ub8t(-ddq[l]+ddq[2] ,ddq[4] ,fc[2] ) ; 

(dl4)  [(-  gm+lm+gm)  co8(q  )  v 
4  4  2  3  2  2  2 

+  ((ddq  1  g  m  -  ddq  1  g  m  )  8in(q  ) 

1233  1144  1 

2  2 

+  (dq  Igm-dqlgm)  co8(q  ))  8iii(q  ) 

1233  1144  1  2 

2  2 

+  ((dq  1  gm  -dq  1  gm)  8in(q  ) 

1144  1233  1 

2 

+(ddq  1  g  m  -  ddq  1  g  m  )co8(q  ))co8(q  )+  ddq  g  m  +ddq  izz 
1233  1144  1  2  244  2  4 

2  2 

+(ddq  -  ddq  )ia  +  ddq  1  m  ♦(ddq  -ddq  )ia  +  ddq  g  m  +  ddq  izz 
2  14  223  2  13  222  22 

♦  ddq  ia  ] 

2  2 

(cl5)  fc[l] :fullrat8imp(fc[l] ) ; 

(dl5)  [(1  m  +g  m  +g  m)  co8(q  )  v 
1  4  3  3  1  1  1 
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+  ((ddq  1  g  m  -  1  ddq  g  m  )  siii(q  ) 

2233  1  244  1 

2  2 

+  (1  dq  g  m  -  dq  1  g  m  )  cosCq  ))  sin(q  ) 

1244  2233  1  2 

2  2 

+  ((dq  Igm-ldqgm)  sin(q  ) 

2233  1244 

2 

+  (ddq  1  g  m  -  1  ddq  g  m  )  cos(q  >)  co8(q  )  +  ddq  1  m 
2233  1  244  1  2  114 

2  2 
-•'(ddq  -ddq  )ia  -•ddq  g  m  -•ddq  izz  -•(ddq  -ddq  )ia  -•ddq  g  m 

1  24  133  13  1  23  111 

-•  ddq  izz  -•  ddq  ia  ] 

11  11 

(cl6)  fc[2] :lullrat8imp(fc[2] ) ; 

(dl6)  [(-  gm-^lm-^gm)  co8(q  )  v 
4  4  2  3  2  2  2 

■•  ((ddq  1  g  m  -  ddq  1  g  m  )  sin(q  ) 

1233  1144  1 

2  2 

-•  (dq  1  gm  -  dq  1  gm)  cos(q  ))  8in(q  ) 

1233  1144  1  2 


2  2 

-•  ((dq  Igm-dqlgm)  8in(q  ) 

1144  1233  1 

2 

-•(ddq  1  g  m  -ddq  1  g  m  )co8(q  ))co8(q  )-•  ddq  g  ra  -•ddq  izz 
1233  1144  1  2  244  24 
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2  2 

-i-Cddq  -  ddq  )ia  -i-ddq  1  m  -•'(ddq  -ddq  )ia  ddq  g  m  -*■  ddq  izz 
2  14223  2  13  222  2  2 

-*■  ddq  ia  ] 

2  2 

(cl7)  dll:ratcoeff (fc[l] ,ddq[l]  ) ; 

2  2  2 
(dl7)  [1  m  +  ia  +  g  m  +  izz  +  ia  +  g  m  +  izz  +  ia  ] 

14  433  3  311  1  1 

(cl8)  dl2:ratcoaff (fc[l] ,ddq[2]  ) ; 

(dl8)  [(1  g  m  -  1  g  m  )  sinCq  )  sinCq  ) 

233  144  1  2 

+  (lgin-lgni)  cos(q  )  cosCq  )  -  ia  -  ia  ] 
233144  1  2  4  3 

(cl9)  d21 :ratco«lf (f c[2] ,ddqCl]  ) ; 

(dl9)  [(1  g  m  -  1  g  m  )  sinCq  )  ainCq  ) 

233  144  1  2 

+(lgm-lgm)  coB(q  )  cosCq  )  -  ia  -  ia  ] 
233144  1  2  4  3 

(c20)  d22:ratco«ff (fc[2] ,ddq[2]); 

2  2  2 

(d20  [g  m  +  izz  +ia  +1  m  +ia  +  g  m  +  izz  +  ia  ] 

44  4  423  322  2  2 

(c21)  gl:ratcoalf (fc[l] ,v) ; 

(d21)  [(1  m  +g  m  +g  m)  cosCq  )] 

1  4  3  3  1  1  1 

(c22)  g2:ratcoalf (fc[2]  ,v) ; 
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(d22) 


[(-g  m  +1  m  +  g  m)  co8(q  )] 

4  4  2  3  2  2  2 

(c23)  hlll’.ratcoeff  (fc[l]  ,dq[l]  ,2)  ; 

(d23)  [0] 

(c24)  hll2:ratcoeff (fc[l] ,dq[l] ,1) : 

(d24)  [0] 

(c25)  hl22:ratcoaff (fcCl] ,dqC23 ,2) ; 

(d25)  [(1  g  m  -  1  g  m  )  cosCq  )  ainCq  ) 
144  233  1  2 

+  (1  gm  -1  gni)  8iii(q  )  co8(q  )] 
233  144  1  2 

(c26)  h211 :ratcoaff (fc[23 ,dq[l]  ,2) ; 

(d26)  [(1  g  m  -  1  g  ®  )  co8(q  )  sinCq  ) 
233  144  1  2 

+(lgm-lgra)  8in(q  )  co8(q  )] 
144  233  1  2 

(c27)  h212:ratcoaff (fc[2] ,dq[l] ,1) ; 

(d27)  [0] 

(c28)  h222:ratco8ff (fc[2] ,dq[2] ,2) ; 

(d28)  [0] 

(c29)  8ava("fcpbarr8dtic8d",fc) ; 

(d29)  [Icpbarraducad,  fc] 

(c30)  clo88fila(); 
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A.i  Conchision 


The  Leu  and  Hemati,[9],  macsyma  program  has  been  modified  to  compute 
the  equations  of  motion  for  closed  link  kinematic  chziin  manipulators  using  the 
aJgorithm  as  presented  by  Luh  and  Zheng,  [10].  However,  the  method  of  extending 
this  program  to  manipulators  that  have  closed  link  kinematic  chains  in  combina¬ 
tion  with  open  link  kinematic  chains  has  not  been  completely  explored.  In  J.  F. 
Kleinfinger  and  W.  Khahil’s  paper  “Dynamic  Modelling  of  Closed-Loop  Robots”, 
[7],  they  state  that  they  too  tried  to  implement  Luh  and  Zheng’s  algorithm  using  a 
Denavit-Hartenburg  convention,  but  had  to  develop  a  new  geometric  representation 
convention  to  make  their  software  work. 

Further  research  into  the  work  of  Kleinfinger  and  Khahil  could  enhance 
AFIT’s  capability  to  model  robotic  manipulator  configurations  greatly. 
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Appendix  B.  MatrixX  Programming 

For  the  numerical  integration  of  the  work  expressions  the  following  MatrixX 
program,  [16],  was  used. 

This  is  a  typical  Vax  management  system  command  file  used  to  submit  nu¬ 
merical  integration  jobs  in  batch  mode.  It  calls  up  initial  conditions  needed  to 
start  the  program  and  calculates  the  work  for  a  large  section  of  reachable  points 
in  the  first  quadrant  of  the  workspace.  Once  the  work  is  determined,  the  efficiency 
is  then  computed. 

tmH8 

define  ’atan2slp.matx’ 
executeC'icconinvar .matx") ; 
for  i=l ;36; .  . . 

k(i)=0.02*i; . . . 
for  j=9:20: . . . 
yf=(j*0.7/20.0)j... 
yi=yf+0.1j . . . 
yaxi«(j)=yi; . . . 
lnt=20; . . . 

executeC'lnitialize.matx") ; . . . 
executeC'lntegrate.matx") ; . . . 

etae(l,j)=ab8(mo«a*(yf-yi))/(aba(vorkls(i,j))'faba(vork2a(i,J))) ; . . . 
etapCi, j)=aba(moea*(yf-yl))/(aba(vorklp(i, j))'faba(«ork2p(i, j))) ; . . . 
etaprime(l,j)=etaa(l,j)/atap(i,j) ; . . . 

end ; . . . 

i=l+l;... 

end; 

for  isl :20; . . . 

k(l)=0.02*i: . . . 
for  j«20:26: . . . 
yf=(je0.r/20.0);... 
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yi=yf+0. 1 i . . . 
yMi»(j)=yi:... 
int=20: . . . 

executaC'initialize.matx") ; _ 

axecut«("int«grate.matx") ; _ 

•taad ,  j  )=abs(mo«a*(yl-yi)  )/(ab8(Horkls(i,  j)  }-t’abs(Hork28(i ,  j)) ) 
•tap(i ,  j)=ab8(mo*a*(yf-yi)  )/(ab8(worklp(i ,  j )  )'*^ab8(Hork2p(i,  j ))) 

etaprime ( i , j ) =ata8 (i,j)/atap(i,j); _ 

j=j  +  l: .  . . 

•nd ; . . . 
i=i+l :  .  .  . 
and; 

lor  i=17;36; .  .  . 

k(i)=0.02*i: . . . 
for  j=l :9: . .  . 
yf=(j*0.7/20.0):... 
yi=yf+0.1: . . . 
yaxi8(j)=yi: . . . 
int=20; . . . 

axacutaC'initializa.matx") ; _ 

axacutaC'lntagTata.matx") ; . . . 

ataaCi, j)=ab8(mo*a*(yf-yi))/(ab8(«orkl8(i, j))-fab8(vork28(i, j))) 
atapCl,  j)=ab8(mo*a*(yf-yl))/(ab8(«orklp(i,  j))-'-ab8(«ork2p(l,  j))) 
ataprima(i,j)=ata8(i, j)/atap(i, j) ; . . . 

and ; . . . 
i=i+l: . . . 
and; 

for  1=36:43; . .  . 

k(i)=0.02*i; . . . 
for  j=l ;12; . . . 
yl=(j*0. 7/20.0);... 
yi=yf+0.1; . . . 
yaxl8(j)=yi:... 
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int=20; . . . 

•x«cut«("initialize.matx") ; . . . 
axecutaC'intagrata.matx") ; . . . 

atas(i , j )=ab8(mo*a*(yf-yi) )/(ab8(workls(i , j ) )'fab8(«ork28(i ,  j))):... 
atap(i,j)=ab8(mo*a*(yf-yi))/(ab8(worklp(i,j))+ab8(work2p(i, j))) ; . . . 

•taprima(i , j)=ata8{i , j )/atap(i, j) ; . . . 

end ;  .  .  . 
i=i+l:  .  .  . 
end; 

save  ’ inert us .ndat ’ 

exit 

$exit 

This  is  the  end  of  the  main  program.  The  subroutines  called  by  this  command  file 
are  atan2slp.matx,  icconinvar.matx,  initialize.matx,  and  integrate.matx.  These  will 
all  be  explained  as  they  are  presented.  The  first  is  atan2slp.matx.  This  subroutine 
requires  two  arguments,  x  and  y  position  in  the  workspace,  and  finds  the  arctangent 
resolving  any  quadrant  ambiguity  that  naturally  arises  using  the  not  well-behaved 
inverse  tangent  function.  This  is  the  same  as  the  atan2  function  in  Fortran. 

//theta=8tan28lp(num,d«n) ; 
th«ta=atan(nu]n/d«n) ; 
if  dan<0  then; . . . 

if  nun>0  then;  theta=pi'^theta;  end;... 
if  num<0  then;  theta=theta-pi ;  end;... 
end; 

if  den=0  then; . .  . 

if  nuin>0  then;  theta=pi/2;  end;... 
if  nunxO  then;  theta=-pi/2;  end;... 
end; 
retf ; 

The  next  subroutine  called  is  icconinvar.matx.  This  file  initializes  a  number  of 
constants  used  in  the  equations  of  motion.  These  consist  mainly  of  link  lengths 
and  masses.  This  file  is  as  follows, 
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a=9.8; 
mo=l . 6 ; 
g=0; 
c=.2: 

ml=mo/(12*c) ; 
m2=mo/(6*c) ; 

11=. 36322; 

12=.6705; 
iiun= .  75*mo/ c 

llcB  =  (-ino*ll-m2*ll)/ml ; 

12c=(-mo*12)/m2 ; 

13=11; 

14=12; 
m3=iBl  ; 
m4=m2 ; 
iB2p=in2/4; 

12p=12/4; 

12pc=(ino*14-ni3*12)/m2; 

14c=0; 

13c=(ino*ll*14)/(m3*12p) ; 

llcp=(-ll*(mo+m4)-iB3*13c)/ml ; 

irzl=.000072917*inl; 

izr2=.  000072917*1112; 

izz3=.000072917*m3; 

lzz4=.  000072917*184: 

izz2p= . 000072917*m2p ; 

The  next  subroutine  called  is  initialize. matx.  This  routine  initializes  the  integration 
routine  by  calculating  the  first  quantity  of  the  arrays.  Therefore,  when  integrate 
takes  the  difference  of  the  i-1  quantity  in  the  array,  it  can.  This  file  is  as  follows, 

y(i)=yi: 

t(l)=0: 

worklsd,  j)=0; 

«ork2a(i,j)=0; 
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Borklpd,  j)=0: 
worlt2p(l,j)=0: 

th2(l)=aco8(((k(i)**2)+(y(l)**2)-(ll**24l2**2))/(2*lI*12)) ; 
nuin=((-(12*«in(th2(l)))*k(i))+((ll+12*C08(th2(l)))*y(l))); 
den=((12*8in(th2(l))*y(l))+((ll+12*co8(th2(l)))*k(i))); 
thl  ( 1  )=atan28lp(nuai,d«n) ; 
ydot(l)=-a*t(l) ; 

thldot(l)=(12*8in(thl(l)+th2(l))*ydot(l))/(ll*12*8in(th2(l))) : 

th2dot ( 1 ) = ( ( -ll*8in(thl ( 1) ) -12*8in(thl ( 1 ) +th2 ( 1 ) ) ) *ydot ( 1 ) ) / . . . 
(Il*12*8in(th2(l))); 

yddot=-a: 

thlddot(l)=(yddot+12*8in(thl(l)+th2(l))*(thldot(l)+th2dot(l))**2+. . . 

(Il*co8(thl(l))*(thldot(l)**2)+12*co8(thl(l)+th2(l))*. . . 
(thldot(l)+th2dot(l))**2)*(cot(thl(l)+th2(l)))+. . . 
ll*8in(thl(l))*thldot(l)**2)/(H*co8(thl(l))+12*. . . 
co8(thl(l)+th2(l))-cot(thl{l)+th2(l))*(ll*8in(thl(I))+. , , 
I2*8in(thl(l)+th2(l)))): 

th2ddot(l)=(ll*8in<thl(l))*thlddot(l)+ll*co8(thl(l))*thldot(l)**24-. . . 

12*8in(thl(l)+th2(l))*thlddot(l)+12*co8(thl(l)+th2(l))*.  . 
(thldot(l)+th2dot(l))**2)/(-12*8iii(thl(l)+th2(l))); 
tl8(l)=(nil*llc8**2+irzl+mlpr*llprc**2+iraipr+m2pr*12prc**2. . . 
+min*12pr**2+izz2pr+mB*llpr**2. . . 

+  (m2+niiB+m2pr)*ll*«2+m2*12c**2+izz2+mo*ll**2+BC*12**2. . . 

+2*H* .  . . 

(-Bm*12pr-ii2pr*12prc+m2*12c+Bio*12)*co8(th2(l)))*thlddot(l) . . . 
+(B2*12c**24izz2+m2pr*12prc**2+izz2pr+wB*12pr**2+Bo*12**2. . . 
+ll*(-mB*12pr-B2pr*12prc+B2*12c+mo»12)*co8(th2(l)))*th2ddot(l) 
+(-Bm*12pr-Bi2pr*12prc+B2*12c+iBo*12)*g*co8(thl(l)+th2(l)). . . 

♦  (Bl*llC8+(Bi2+BB+m2pr)*ll+Bo*ll-Blpr*llprc-mni*llpr) . . . 
♦g*co8(thI(l)) : 

t28(l)=(m2*12c**2+izz2+m2pr*12prc^*2+izz2pr+min*12pr**2. . . 

+bo*12^*2+1 1 ♦ ( -BB*12pr-B2pr*12prc+B2*12c+mo*12 ) *008 ( th2 ( 1 ) ) ) * . 
thlddot(l)+(B2*12c**2+izz2+B2pr*12prc**2+izz2pr+. . . 
BO*12**2+BB*12pr**2)*th2ddot(l) . . . 
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+ ( -min*12pr-m2pr*12prc+m2*12c+mo*12 ) *g*co8 (thl ( 1 ) +th2 ( 1 ) ) : 
tlp(l)  =  (ml*llpc**2+izsl+mlppr*llpprc**2+ij:zlppr+iiim*llppr**2.  .  . 

+m2p*ll**2 . . . 

+m3*13c**2+izz3+m4*ll**2+nio*ll**2)*thlddot(l) . . . 
-(m2p*ll*12pc+m3*12p*13c-ni4*ll*14c-mo*ll*14) . . . 
•co8(th2(l))*(thlddot(l)+. . . 

th2ddot(l))  +  (inl*llpc+iB3*13c+(iii4+in2p)*ll+mo*ll-am*llppr .  . . 
-mlppr*llpprc) . . . 

*g*co8(thl(l)) : 

t2p(l)=-(m3*12p*13c-ni4*ll*14c-ino*ll*14+m2p*ll*12pc . .  . 

)*co8(th2(l) )*thlddot(l) . . . 
+(m2»12pc**2+izz2p+m3*12p**2+mm*12ppr**2+. . . 
in4*14c**2+izz4+mo*14**2)*(thlddot(l) . . . 
+th2ddot(l))-(2*m2p*12pc+in3*12p+inm*12ppr. . . 
-m4*14c-mo*14)*g*co8(thl(l) . . . 

♦th2(l)) ; 

Finally,  the  numerical  integration  is  able  to  be  executed.  This  routine  is  in  inte¬ 
grate. matx  and  is  as  follows, 

for  n=2 :21 ; . . . 

t(n)=(8qrt((2*(ab8(yf-yi)))/*))*(n-l)/intj . . . 
y(n)=yi-(«*(t(n)**2)/2) ; . . . 

th2(n)=aco8(((k(i)**2)+(y(n)**2)-(ll**2+12**2))/(2*ll*12))j . . . 
nuin=((-(12*8in(th2(n)))*k(i))+((ll+12*co8(th2(n)))*y(n))) ; . . . 
dan={(12*8in(th2(n))*y(n))+((ll+12*co8(th2(n)))*k(i))); . . . 
thl(n)=atan28lp(num,d«n) ; . . . 
ydot(n)=-a*t(n) ; . . . 

thldot(n)  =  (12*8in(thl(n)+th2(n))*ydot(n))/(ll*12*8in(th2(n))) ; . . . 
th2dot(n)=((-ll*8ln(thl(n))-12*8in(thl(n)+th2(n)))*ydot (n))/. . . 

(Il*128sin(th2(n))); . . . 
yddot=-a; . . . 

thlddot(n)=(yddot+12*8in(thl(n)+th2{n))*(thldot(n)+th2dot(n))**2+. . . 

(ll*co8(thl(n))*(thldot(n)**2)+12*co8(thl(n)+th2(n))*. . . 
(thldot(n)>th2dot(n))**2)*(cot(thl(n)+th2(n)))+. . . 


ll*«in(thl (n))*thldot(n)**2)/ (ll*co8(thl (n))+12* . . . 
co8(thl(n)+th2(n))-cot(thl(n)+th2(n))*(ll*«in(thl(n))+. . . 
12*8in(thl(n)-t'th2(n)))) ;  — 

th2ddot(n)=(ll*8in(thl(n))*thlddot(n)+ll*co8(thl(n))*thldot(n)**2+ . . . 

12*8in( thl (n) +th2 (n) ) •thlddot (n) +12*co8 ( thl (n) +th2 (n) ) * . . . 
(thldot(n)+th2dot(n))**2)/(-12*8ln(thl(n)+th2(n))) ; . . . 
tl8(n)  =  (ml*llc8**2+izzl+iiilpr*llprc**2+izzlpr+m2pr*12prc**2. . . 
+mm*12pr**2+izz2pr+min*llpr**2 . . . 

+  (m2+inin+m2pr)*ll**2+m2*12c**2+izz2+mo*ll**2+mo'»12**2. . . 
+2*11*. .  . 

(-nun*12pr-m2pr*12prc+m2*12c+rao*12)*co8(th2(n)))*thlddot(n) . . . 

+  (m2*12c**2+izz2+m2pr*12prc**2+izz2pr+iiim*12pr**2+mo*12**2. . . 
+ll*(-imii*12pr-m2pr*12prc+m2*12c+mo*12)*co8(th2{n)))*th2ddot(n) 
+  (-iiiin*12pr-in2pr*12prc+m2*12c+mo*12)*g*co8(thl(n)+th2(n)) .  .  . 

+  (ml*llc8+(m2+mm+m2pr)*ll+BO*ll-mlpr*llprc-iiuB*llpr) . . . 
•g*co8(thl(n)) ; . , . 

t28(n)=(m2*12c**2+izz2+B2pr*12prc**2+izz2pr+miB*12pr**2. . . 

+mo*12**2+ll*(-«m*12pr-iB2pr*12prc+m2*12c+BO*12)*co8(th2(n)))*. 
thlddot (n) + (B2*12e**2+izz2+B2pr*12prc**2+izz2pr+ . . . 
Bio*12**2+miB*12pr**2)*th2ddot(n) . . . 

+  (-Biin*12pr-m2pr*12prc+m2*12c+Bo*12)*g*co8(thl(n)+th2(n)) : . . . 
tlp(n)=(Bl*llpc**2+izzl+mlppr*llpprc**2+izzlppr+BB*llppr**2. . . 
+m2p*ll**2. . . 

+m3*13c**2+izz3+B4*ll**2+BO*ll**2)*thlddot(n) . . . 
-(■2p*ll*12pc+B3*12p*13c-Bi4*ll*14c-BO*ll*14) . . . 
*co8(th2(n))*(thld(  otCn)-*-. . . 

th2ddot(n))+(Bl*llpc+B3*13c+(B4+B2p)*ll+BO*ll-BB*llppr. . . 
-Blppr*llpprc) . . . 

*g*co8(thl(n)) : . . . 

t2p(n)=-(B3*12p*13c-B4*ll*14c-BO*ll*14+B2p*ll*12pc. . . 

)*co8(th2(n) )*thlddot(n) — 
•••(B2*12pc**2+izz2p+B3*12p**2+BB*12ppr**2+. . . 
B4*14c**2+izz4+Bo*14**2)*(thlddot(n) . . . 
'«’th2ddot(n))-(2*B2p*12pc'*^B3*12p+BB*12ppr. . . 
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-m4*14c-mo*14) *g*co«(thl (n) . . . 

+th2(n)) : . . . 

wl«(n)=.B*(tl8(n-l)+tl8(n))*(thl(n)-thl(n-l)); . . . 
w28(n)= .6*(t28(n-l)+t28(n))*(th2(n)-th2(n-l)) ; — 
Hlp(n)=.B*(tlp(n-l)+tlp(n))*{thl(n)-thl(n-l)); . . . 

i»2p(n)=.B*(t2p(n-l)+t2p(n))*((thl(n)+th2(n))-{thl(n-l)+th2(n-l))) ; . . . 
workl8(i , j )=workl8(i , j )+wl8(n) ; . . . 
work28(i,j)=work28(i,j)-(-w28(n): . . . 
worklp(i,j)=worklp(i,j)+wlp(n): . . . 

Hork2p(i , j )=Hork2p(i , j )+w2p(n) ; — 
n=n+l ; . .  . 
end ; . .  . 

These  are  the  algorithms  used  to  perform  the  numerical  integration  of  work  ex¬ 
pression  for  this  analysis. 
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